import ctypes as ct
from ctypes import c_byte, c_char_p, c_double, c_int, c_ubyte, c_uint, c_ushort
from typing import Tuple
import typing_extensions
import forcedimension_core.runtime as _runtime
from forcedimension_core.constants import MAX_DOF, ComMode, DeviceType
from forcedimension_core.typing import (
Array, FloatDOFTuple, MutableArray,
c_double_ptr, c_int_ptr, c_ubyte_ptr, c_uint_ptr, c_ushort_ptr
)
from . import direct as direct
_runtime._libdhd.dhdEnableExpertMode.argtypes = []
_runtime._libdhd.dhdEnableExpertMode.restype = c_int
[docs]def enableExpertMode() -> int:
"""
Enable expert mode.
:returns: 0 on success, -1 otherwise.
See Also
--------
| :func:`forcedimension_core.dhd.expert.disableExpertMode()`
"""
return _runtime._libdhd.dhdEnableExpertMode()
_runtime._libdhd.dhdDisableExpertMode.argtypes = []
_runtime._libdhd.dhdDisableExpertMode.restype = c_int
[docs]def disableExpertMode() -> int:
"""
Disable expert mode.
:returns: 0 on success, -1 otherwise.
See Also
--------
| :func:`forcedimension_core.dhd.expert.disableExpertMode()`
"""
return _runtime._libdhd.dhdDisableExpertMode()
_runtime._libdhd.dhdPreset.argtypes = [c_int_ptr, c_ubyte, c_byte]
_runtime._libdhd.dhdPreset.restype = c_int
[docs]def preset(val: Array[int, int], mask: int = 0xff, ID: int = -1) -> int:
"""
Set selected encoder offsets to a given value. Intended for
use with the generic controller when no RESET button is
available.
:param Array[int, int] val:
Vector of encoder offsets refering to each DOF with length
:data:`forcedimension_core.constants.MAX_DOF`.
:param int mask:
Bitwise mask of which encoder should be set.
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:raises ctypes.ArgumentError:
If ``val`` is not implicitly convertible to a C char.
:raises IndexError:
If ``len(val) < MAX_DOF``.
:raises TypeError:
If ``val`` is not subscriptable.
:raises ctypes.ArgumentError:
If ``mask`` is not implicitly convertible to a C char.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns: 0 on success, -1 otherwise
"""
vals = (c_int * MAX_DOF)(
val[0], val[1], val[2], val[3], val[4], val[5], val[6], val[7]
)
return _runtime._libdhd.dhdPreset(
ct.cast(vals, c_int_ptr),
mask,
ID
)
_runtime._libdhd.dhdSetTimeGuard.argtypes = [c_int, c_byte]
_runtime._libdhd.dhdSetTimeGuard.restype = c_int
[docs]def setTimeGuard(min_period: int, ID: int = -1) -> int:
"""
Enable/disable the :ref:timeguard: feature with an arbitrary
minimum period.
:param int min_period:
Minimum refresh period (in [us]). A value of 0.
disables the TimeGuard feature, while a value of -1 resets the default
recommended value.
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:raises ctypes.ArgumentError:
If ``min_period`` is not implicitly convertible to a C char.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns: 0 on success, -1 otherwise.
"""
return _runtime._libdhd.dhdSetTimeGuard(min_period, ID)
_runtime._libdhd.dhdSetVelocityThreshold.argtypes = [c_uint, c_byte]
_runtime._libdhd.dhdSetVelocityThreshold.restype = c_int
[docs]def setVelocityThreshold(thresh: int, ID: int = -1) -> int:
"""
Adjust the :ref:`velocity_threshold` of the device (in [m/s]).
The velocity threshold is a safety feature that prevents
the device from accelerating to high velocities without
control. If the velocity of one of the device axis passes the
threshold, the device enters BRAKE mode.
Warning
-------
Since the range of threshold values is device dependent, it is
recommended NOT to modify factory settings.
:param int thresh:
An arbitrary value of velocity threshold (in [m/s]).
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:raises ctypes.ArgumentError:
If ``thresh`` is not implicitly convertible to a C char.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns: 0 on success, -1 otherwise.
See Also
--------
| :func:`forcedimension_core.dhd.expert.getVelocityThreshold()`
"""
return _runtime._libdhd.dhdSetVelocityThreshold(thresh, ID)
_runtime._libdhd.dhdGetVelocityThreshold.argtypes = [c_uint_ptr, c_byte]
_runtime._libdhd.dhdGetVelocityThreshold.restype = c_int
[docs]def getVelocityThreshold(ID: int = -1) -> int:
"""
Get the velocity threshold of the device. The velocity
threshold is a safety feature that prevents the device from
accelerating to high velocities without control. If the
velocity of one of the device axis passes the threshold, the
device enters BRAKE mode.
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns:
The non-negative velocity threshold on success, -1 otherwise.
See Also
--------
| :func:`forcedimension_core.dhd.expert.setVelocityThreshold()`
"""
thresh = c_uint()
if _runtime._libdhd.dhdGetVelocityThreshold(thresh, ID):
return -1
return thresh.value
_runtime._libdhd.dhdUpdateEncoders.argtypes = [c_byte]
_runtime._libdhd.dhdUpdateEncoders.restype = c_int
[docs]def updateEncoders(ID: int = -1) -> int:
"""
Force an update of the internal encoder values in the state
vector. This call retrieves the encoder values from the
device and places them into the state vector. No kinematic
model is called.
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns: 0 on success, -1 otherwise.
See Also
--------
| :func:`forcedimension_core.dhd.expert.getEnc()`
"""
return _runtime._libdhd.dhdUpdateEncoders(ID)
_runtime._libdhd.dhdGetDeltaEncoders.argtypes = [
c_int_ptr, c_int_ptr, c_int_ptr,
c_byte
]
_runtime._libdhd.dhdGetDeltaEncoders.restype = c_int
[docs]def getDeltaEncoders(out: MutableArray[int, int], ID: int = -1) -> int:
"""
Read all encoders values of the DELTA structure.
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:param Array[int, int] out:
An output buffer to store the raw encoder values.
:raises TypeError:
If ``out`` does not support item assignment either
because it's immutable or not subscriptable.
:raises IndexError:
If ``len(out) < 3``.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns:
0 or :data:`forcedimension_core.constants.TIMEGUARD`
on success, and -1 otherwise.
See Also
--------
| :func:`forcedimension_core.dhd.expert.deltaEncodersToJointAngles()`
| :func:`forcedimension_core.dhd.expert.deltaEncoderToPosition()`
"""
enc0 = c_int()
enc1 = c_int()
enc2 = c_int()
err = _runtime._libdhd.dhdGetDeltaEncoders(enc0, enc1, enc2, ID)
out[0] = enc0.value
out[1] = enc1.value
out[2] = enc2.value
return err
_runtime._libdhd.dhdGetWristEncoders.argtypes = [
c_int_ptr, c_int_ptr, c_int_ptr,
c_byte
]
_runtime._libdhd.dhdGetWristEncoders.restype = c_int
[docs]def getWristEncoders(out: MutableArray[int, int], ID: int = -1) -> int:
"""
Read all encoders values of the WRIST structure.
Note
----
This feature only applies to devices with a wrist. See
the :ref:`device_types` section for more details.
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:param Array[int, int] out:
An output buffer to store the raw wrist encoder values.
:raises TypeError:
If ``out`` does not support item assignment either
because it's immutable or not subscriptable.
:raises IndexError:
If ``len(out) < 3``.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns:
0 or :data:`forcedimension_core.constants.TIMEGUARD` on success,
-1 otherwise.
See Also
--------
| :func:`forcedimension_core.dhd.wristEncodersToOrientation()`
| :func:`forcedimension_core.dhd.wristEncodersToJointAngles()`
| :func:`forcedimension_core.dhd.wristOrientationToEncoder()`
"""
enc0 = c_int()
enc1 = c_int()
enc2 = c_int()
err = _runtime._libdhd.dhdGetWristEncoders(
enc0,
enc1,
enc2,
ID
)
out[0] = enc0.value
out[1] = enc1.value
out[2] = enc2.value
return err
_runtime._libdhd.dhdGetGripperEncoder.argtypes = [c_int_ptr, c_byte]
_runtime._libdhd.dhdGetGripperEncoder.restype = c_int
[docs]def getGripperEncoder(out: c_int, ID: int = -1) -> int:
"""
Read the encoder value of the force gripper.
Note
----
This feature only applies to devices with a gripper. See
the :ref:`device_types` section for more details.
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:param c_int out:
Output buffer to store the encoder value of the force gripper.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns:
0 or :data:`forcedimension_core.constants.TIMEGUARD`
on success, -1 otherwise.
"""
return _runtime._libdhd.dhdGetGripperEncoder(out, ID)
_runtime._libdhd.dhdGetEncoder.argtypes = [c_int, c_byte]
_runtime._libdhd.dhdGetEncoder.restype = c_int
[docs]def getEncoder(index: int, ID: int = -1) -> int:
"""
Read a single encoder value from the haptic device.
:param int index:
The motor index number as defined by
:data:`forcedimension_core.constants.MAX_DOF`
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:raises ctypes.ArgumentError:
If ``index`` is not implicitly convertible to a C char.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns: The (positive) encoder reading on success, -1 otherwise.
See Also
--------
| :func:`forcedimension_core.dhd.expert.getEnc()`
"""
return _runtime._libdhd.dhdGetEncoder(index, ID)
_runtime._libdhd.dhdSetMotor.argtypes = [c_int, c_ushort, c_byte]
_runtime._libdhd.dhdSetMotor.restype = c_int
[docs]def setMotor(index: int, output: int, ID: int = -1) -> int:
"""
Program a command to a single motor channel.
:param int index:
The motor index number as defined by
:data:`forcedimension_core.constants.MAX_DOF`
:param int output:
The motor DAa C char.
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:raises ctypes.ArgumentError:
If ``index`` is not implicitly convertible to a C char.
:raises ctypes.ArgumentError:
If ``output`` is not implicitly convertible to a C char.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns: 0 on success, -1 otherwise.
See Also
--------
| :func:`forcedimension_core.dhd.expert.setMot()`
| :func:`forcedimension_core.dhd.expert.setDeltaMotor()`
| :func:`forcedimension_core.dhd.expert.setWristMotor()`
"""
return _runtime._libdhd.dhdSetMotor(index, output, ID)
_runtime._libdhd.dhdSetDeltaMotor.argtypes = [
c_ushort, c_ushort, c_ushort, c_byte]
_runtime._libdhd.dhdSetDeltaMotor.restype = c_int
[docs]def setDeltaMotor(mot: Array[int, int], ID: int = -1) -> int:
"""
Set desired motor commands to the amplifier channels
commanding the DELTA motors.
:param Array[int, int] mot:
Sequence of ``(mot0, mot1, mot2)`` where ``mot0``, ``mot1``,
and ``mot2`` are the axis 0, 1, and 2 DELTA motor commands,
respectively.
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:raises ctypes.ArgumentError:
If any element of ``mot`` is not implicitly convertible to a C ushort.
:raises IndexError:
If ``len(mot) < 3``.
:raises TypeError:
If ``mot`` is not subscriptable.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns: 0 on success, -1 otherwise.
See Also
--------
| :func:`forcedimension_core.dhd.expert.setMot()`
| :func:`forcedimension_core.dhd.expert.setMotor()`
| :func:`forcedimension_core.dhd.expert.setWristMotor()`
"""
return _runtime._libdhd.dhdSetDeltaMotor(mot[0], mot[1], mot[2], ID)
_runtime._libdhd.dhdSetWristMotor.argtypes = [
c_ushort, c_ushort, c_ushort,
c_byte
]
_runtime._libdhd.dhdSetWristMotor.restype = c_int
[docs]def setWristMotor(output: Array[int, int], ID: int = -1) -> int:
"""
Set desired motor commands to the amplifier channels
commanding the WRIST motors.
Note
----
This feature only applies to the following devices:
:data:`forcedimension_core.constants.DeviceType.SIGMA7_RIGHT`
:data:`forcedimension_core.constants.DeviceType.SIGMA7_LEFT`
:param Array[int, int] output:
Sequence of (output0, output1, output2) where
``output0``, ``output1``, and ``output2`` are the axis
0, 1, and 2 wrist motor commands, respectively.
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:raises ctypes.ArgumentError:
If any element of ``output`` is not implicitly convertible to a C char.
:raises IndexError:
If ``len(output) < 3``.
:raises TypeError:
If ``output`` is not subscriptable.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns: 0 on success, -1 otherwise.
See Also
--------
| :func:`forcedimension_core.dhd.setMot()`
| :func:`forcedimension_core.dhd.setMotor()`
| :func:`forcedimension_core.dhd.setDeltaMotor()`
| :func:`forcedimension_core.dhd.setGripperMotor()`
"""
return _runtime._libdhd.dhdSetWristMotor(output[0], output[1], output[2], ID)
_runtime._libdhd.dhdSetGripperMotor.argtypes = [c_ushort, c_byte]
_runtime._libdhd.dhdSetGripperMotor.restype = c_int
[docs]def setGripperMotor(output: int, ID: int = -1) -> int:
"""
Set desired motor commands to the amplifier channels
commanding the force gripper.
:param int output:
Gripper motor command.
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:raises ctypes.ArgumentError:
If ``output`` is not implicitly convertible to a C ushort.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns: 0 on success, -1 otherwise.
See Also
--------
| :func:`forcedimension_core.dhd.setMot()`
| :func:`forcedimension_core.dhd.setMotor()`
| :func:`forcedimension_core.dhd.setDeltaMotor()`
| :func:`forcedimension_core.dhd.setWristMotor()`
"""
return _runtime._libdhd.dhdSetGripperMotor(output, ID)
_runtime._libdhd.dhdDeltaEncoderToPosition.argtypes = [
c_int, c_int, c_int,
c_double_ptr, c_double_ptr, c_double_ptr,
c_byte
]
_runtime._libdhd.dhdDeltaEncoderToPosition.restype = c_int
[docs]def deltaEncoderToPosition(
enc: Array[int, int],
out: MutableArray[int, float],
ID: int = -1
) -> int:
"""
Compute and return the position of the end-effector (in [m])
about the X, Y, and Z axes for a given set of encoder values.
:param Array[int, int] enc:
Sequence of ``(enc0, enc1, enc2)`` where ``enc0``,
``enc1``, and ``enc2`` refer to raw encoder values on
axis 0, 1, and 2, respectively.
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:param Array[int, int] out:
An output buffer to store the end-effector position (in [m]).
:raises TypeError:
If ``out`` does not support item assignment either
because it's immutable or not subscriptable.
:raises IndexError:
If ``len(out) < 3``.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:raises ctypes.ArgumentError:
If any element of ``enc`` is not implicitly convertible
to a C int.
:raises IndexError:
If ``len(enc) < 3``.
:raises TypeError:
If ``enc`` is not subscriptable.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns:
0 on success, -1 otherwise.
See Also
--------
| :func:`forcedimension_core.dhd.expert.getDeltaEncoders()`
| :func:`forcedimension_core.dhd.expert.deltaPositionToEncoder()`
| :func:`forcedimension_core.dhd.expert.deltaEncodersToJointAngles()`
"""
px = c_double()
py = c_double()
pz = c_double()
err = _runtime._libdhd.dhdDeltaEncoderToPosition(
enc[0], enc[1], enc[2],
px, py, pz,
ID
)
out[0] = px.value
out[1] = py.value
out[2] = pz.value
return err
_runtime._libdhd.dhdDeltaPositionToEncoder.argtypes = [
c_double, c_double, c_double,
c_int_ptr, c_int_ptr, c_int_ptr,
c_byte
]
_runtime._libdhd.dhdDeltaPositionToEncoder.restype = c_int
[docs]def deltaPositionToEncoder(
pos: Array[int, float],
out: MutableArray[int, int],
ID: int = -1,
) -> int:
"""
Computes and returns the encoder values of the end-effector
for a given end-effector position.
:param Array[int, float] pos:
Sequence of ``(px, py, pz)`` where ``px``, ``py``, and
``pz`` refer to the end-effector position on the X, Y,
and Z axes, respectively (in [m]).
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:param Array[int, int] out:
An output buffer to store the raw encoder values.
:raises TypeError:
If ``out`` does not support item assignment either
because it's immutable or not subscriptable.
:raises IndexError:
If ``len(out) < 3``.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:raises ctypes.ArgumentError:
If any element of ``pos`` is not implicitly convertible
to a C double.
:raises IndexError:
If ``len(pos) < 3``.
:raises TypeError:
If ``pos`` is not subscriptable.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns:
0 on success, -1 otherwise.
See Also
--------
| :func:`forcedimension_core.dhd.expert.getDeltaEncoders()`
| :func:`forcedimension_core.dhd.expert.deltaEncoderToPosition()`
| :func:`forcedimension_core.dhd.expert.deltaEncodersToJointAngles()`
"""
enc0 = c_int()
enc1 = c_int()
enc2 = c_int()
err = _runtime._libdhd.dhdDeltaPositionToEncoder(
pos[0], pos[1], pos[2],
enc0, enc1, enc2,
ID
)
out[0] = enc0.value
out[1] = enc1.value
out[2] = enc2.value
return err
_runtime._libdhd.dhdDeltaMotorToForce.argtypes = [
c_ushort, c_ushort, c_ushort,
c_int, c_int, c_int,
c_double_ptr, c_double_ptr, c_double_ptr,
c_byte
]
_runtime._libdhd.dhdDeltaMotorToForce.restype = c_int
[docs]def deltaMotorToForce(
mot: Array[int, int],
enc: Array[int, int],
out: MutableArray[int, float],
ID: int = -1
) -> int:
"""
Compute and return the force applied to the end-effector for
a given set of motor commands at a given position (defined
by encoder readings)
:param Array[int, int] mot:
Sequence of ``(mot0, mot1, mot2)`` where ``mot0``,
``mot1``, and ``mot2`` are the axis 0, 1, and 2 DELTA
motor commands, respectively.
:param Array[int, int] enc:
Sequence of ``(enc0, enc1, enc2)`` where ``enc0``,
``enc1``, and ``enc2`` refer to encoder values on axis
0, 1, and 2, respectively.
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:param MutableArray[int, float] out:
An output buffer to store the applied force to the end
effector.
:raises TypeError:
If ``out`` does not support item assignment either
because it's immutable or not subscriptable.
:raises IndexError:
If ``len(out) < 3``.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:raises ctypes.ArgumentError:
If any element of ``output`` is not implicitly
convertible to a C ushort.
:raises IndexError:
If ``len(output) < 3``.
:raises TypeError:
If ``output`` is not subscriptable.
:raises ctypes.ArgumentError:
If any element of ``enc`` is not implicitly convertible
to a C char.
:raises IndexError:
If ``len(enc) < 3``
:raises TypeError:
If ``enc`` is not subscriptable.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns:
0 on success, -1 otherwise.
See Also
--------
| :func:`forcedimension_core.dhd.expert.deltaForceToMotor()`
"""
fx = c_double()
fy = c_double()
fz = c_double()
err = _runtime._libdhd.dhdDeltaMotorToForce(
mot[0], mot[1], mot[2],
enc[0], enc[1], enc[2],
fx, fy, fz,
ID
)
out[0] = fx.value
out[1] = fy.value
out[2] = fz.value
return err
_runtime._libdhd.dhdDeltaForceToMotor.argtypes = [
c_double, c_double, c_double,
c_int, c_int, c_int,
c_ushort_ptr, c_ushort_ptr, c_ushort_ptr,
c_byte
]
_runtime._libdhd.dhdDeltaForceToMotor.restype = c_int
[docs]def deltaForceToMotor(
f: Array[int, float],
enc: Array[int, int],
out: MutableArray[int, int],
ID: int = -1
) -> int:
"""
Compute and return the motor commands necessary to obtain a
given force on the end-effector at a given position (defined
by encoder readings).
:param Array[int, float] f:
Sequence of ``(fx, fy, fz)`` where ``fx``, ``fy``, and
``fz`` are the force on the DELTA end-effector on the X,
Y, and Z axes, respectively (in [N]).
:param Array[int, int] enc:
Sequence of ``(enc0, enc1, enc2)`` where ``enc0``,
``enc1``, and ``enc2`` refer to encoder values on axes
0, 1, and 2, respectively.
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:param Array[int, int] out:
An output buffer to store the applied force to the
end-effector.
:raises TypeError:
If ``out`` does not support item assignment either
because it's immutable or not subscriptable.
:raises IndexError:
If ``len(out) < 3``.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:raises ctypes.ArgumentError:
If any element of ``f`` is not implicitly convertible
to a C char.
:raises IndexError:
If ``len(f) < 3``.
:raises TypeError:
If ``f`` is not subscriptable.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns:
0 or :data:`forcedimension_core.constants.MOTOR_SATURATED`
on success, and -1 otherwise.
See Also
--------
| :func:`forcedimension_core.dhd.expert.deltaMotorToForce()`
"""
output0 = c_ushort()
output1 = c_ushort()
output2 = c_ushort()
err = _runtime._libdhd.dhdDeltaForceToMotor(
f[0], f[1], f[2],
enc[0], enc[1], enc[2],
output0, output1, output2,
ID
)
out[0] = output0.value
out[1] = output1.value
out[2] = output2.value
return err
_runtime._libdhd.dhdWristEncoderToOrientation.argtypes = [
c_int, c_int, c_int,
c_double_ptr, c_double_ptr, c_double_ptr,
c_byte
]
_runtime._libdhd.dhdWristEncoderToOrientation.restype = c_int
[docs]def wristEncoderToOrientation(
enc: Array[int, int],
out: MutableArray[int, float],
ID: int = -1,
) -> int:
"""
For devices with a wrist structure, compute the individual
angle of each joint, starting with the one located nearest
to the wrist base plate.
Note
----
For the
:data:`forcedimension_core.constants.DeviceType.OMEGA6_RIGHT`
and the
:data:`forcedimension_core.constants.DeviceType.OMEGA6_LEFT`
devices, angles are computed with respect to their internal
reference frame, which is rotated 45 degrees or π/4 radians
about the Y axis. Please refer to your device user manual for
more information on your device coordinate system.
Note
----
This feature only applies to devices with a wrist. See
the :ref:`device_types` section for more details.
:param Array[int, int] enc:
Sequence of ``(enc0, enc1, enc2)`` where ``enc0``,
``enc1``, and ``enc2`` refer to wrist encoder values on
the first, second, and third joint, respectively
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:param MutableArray[int, float] out:
An output buffer to store the joint angles.
:raises TypeError:
If ``out`` does not support item assignment either
because it's immutable or not subscriptable.
:raises IndexError:
If ``len(out) < 3``.
:raises ctypes.ArgumentError:
If any element of ``enc`` is not implicitly convertible
to a C char.
:raises IndexError:
If ``len(enc) < 3``.
:raises TypeError:
If ``enc`` is not subscriptable.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns:
0 on success, -1 otherwise.
See Also
--------
| :func:`forcedimension_core.dhd.expert.wristEncodersToJointAngles()`
| :func:`forcedimension_core.dhd.expert.wristOrientationToEncoder()`
| :func:`forcedimension_core.dhd.expert.wristJointAnglesToEncoders()`
"""
px = c_double()
py = c_double()
pz = c_double()
err = _runtime._libdhd.dhdWristEncoderToOrientation(
enc[0], enc[1], enc[2],
px, py, pz,
ID
)
out[0] = px.value
out[1] = py.value
out[2] = pz.value
return err
_runtime._libdhd.dhdWristOrientationToEncoder.argtypes = [
c_double, c_double, c_double,
c_int_ptr, c_int_ptr, c_int_ptr,
c_byte
]
_runtime._libdhd.dhdWristOrientationToEncoder.restype = c_int
[docs]def wristOrientationToEncoder(
orientation: Array[int, float],
out: MutableArray[int, int],
ID: int = -1,
) -> int:
"""
For devices with a wrist structure, compute the encoder
values from the individual angle of each joint, starting with
the one located nearest to the wrist plate base.
Note
----
For the
:data:`forcedimension_core.constants.DeviceType.OMEGA6`
and
:data:`forcedimension_core.constants.DeviceType.OMEGA6_LEFT`
devices, angles must be expressed with respect to their
internal reference frame, which is rotated 45 degrees or π/4
radians about the Y axis. Please refer to your device user
manual for more information on your device coordinate system.
Note
----
This feature only applies to devices with a wrist. See
the :ref:`device_types` section for more details.
:param Array[int, float] orientation:
Sequence of ``(oa, ob, og)`` where ``oa``, ``ob``, and
``og`` refer to wrist end effector orientation around
the X, Y, and Z axes, respectively (in [rad]).
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:param Array[int, int] out:
An output buffer to store the encoder values.
:raises TypeError:
If ``out`` does not support item assignment either
because it's immutable or not subscriptable.
:raises IndexError:
If ``len(out) < 3``.
:raises ctypes.ArgumentError:
If any element of ``orientation`` is not implicitly
convertible to a C double.
:raises IndexError:
If ``len(orientation) < 3``.
:raises TypeError:
If ``orientation`` is not subscriptable.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns:
0 on success, -1 otherwise
See Also
--------
| :func:`forcedimension_core.dhd.expert.wristEncodersToJointAngles()`
| :func:`forcedimension_core.dhd.expert.wristEncodersToOrientation()`
| :func:`forcedimension_core.dhd.expert.wristJointAnglesToEncoders()`
"""
enc0 = c_int()
enc1 = c_int()
enc2 = c_int()
err = _runtime._libdhd.dhdWristOrientationToEncoder(
orientation[0], orientation[1], orientation[2],
enc0, enc1, enc2,
ID
)
out[0] = enc0.value
out[1] = enc1.value
out[2] = enc2.value
return err
_runtime._libdhd.dhdWristMotorToTorque.argtypes = [
c_ushort, c_ushort, c_ushort,
c_int, c_int, c_int,
c_double_ptr, c_double_ptr, c_double_ptr,
c_byte
]
_runtime._libdhd.dhdWristMotorToTorque.restype = c_int
[docs]def wristMotorToTorque(
output: Array[int, int],
enc: Array[int, int],
out: MutableArray[int, float],
ID: int = -1
) -> int:
"""
Compute and return the torque applied to the wrist
end-effector for a given set of motor commands at a given
orientation (defined by encoder values).
Note
----
This feature only applies to the following devices:
:data:`forcedimension_core.constants.DeviceType.SIGMA7_RIGHT`
:data:`forcedimension_core.constants.DeviceType.SIGMA7_LEFT`
:param Array[int, int] cmd:
Sequence of ``(cmd0, cmd1, cmd2)`` where ``cmd0``,
``cmd1``, and ``cmd2`` are the axis 0, 1, and 2 DELTA
motor commands, respectively.
:param Array[int, int] enc:
Sequence of ``(enc0, enc1, enc2)`` where ``enc0``,
``enc1``, and ``enc2`` refer to encoder values on axss 0,
1, and 2, respectively.
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:param MutableArray[int, float] out:
An output buffer to store the torques applied to the wrist.
:raises TypeError:
If ``out`` does not support item assignment either
because it's immutable or not subscriptable.
:raises IndexError:
If ``len(out) < 3``.
:raises ctypes.ArgumentError:
If any element of ``cmd`` is not implicitly convertible
to a C ushort.
:raises IndexError:
If ``len(cmd) < 3``.
:raises TypeError:
If ``cmd`` is not subscriptable.
:raises ctypes.ArgumentError:
If any element of ``enc`` is not implicitly convertible
to a C char.
:raises IndexError:
If ``len(enc) < 3``.
:raises TypeError:
If ``enc`` is not subscriptable.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns:
0 on success, -1, otherwise.
See Also
--------
| :func:`forcedimension_core.dhd.expert.wristTorqueToMotor()`
| :func:`forcedimension_core.dhd.expert.wristJointTorquesExtrema()`
"""
tx = c_double()
ty = c_double()
tz = c_double()
err = _runtime._libdhd.dhdWristMotorToTorque(
output[0], output[1], output[2],
enc[0], enc[1], enc[2],
tx, ty, tz,
ID
)
out[0] = tx.value
out[1] = ty.value
out[2] = tz.value
return err
_runtime._libdhd.dhdWristTorqueToMotor.argtypes = [
c_double, c_double, c_double,
c_int, c_int, c_int,
c_ushort_ptr, c_ushort_ptr, c_ushort_ptr,
c_byte
]
_runtime._libdhd.dhdWristTorqueToMotor.restype = c_int
[docs]def wristTorqueToMotor(
t: Array[int, float],
enc: Array[int, int],
out: MutableArray[int, int],
ID: int = -1
) -> int:
"""
Compute and return the wrist motor commands necessary to
obtain a given torque (in [Nm]) on the wrist end-effector at
a given orientation (defined by encoder values).
Note
----
This feature only applies to the following devices:
:data:`forcedimension_core.constants.DeviceType.SIGMA7_RIGHT`
:data:`forcedimension_core.constants.DeviceType.SIGMA7_LEFT`
:param Array[int, float] t:
Sequence of ``(t0, t1, t2)`` where ``t0``, ``t1``, and
``t2`` are the DELTA axis torque commands for axes 0, 1, and
2, respectively (in [Nm]).
:param Array[int, int] enc:
Sequence of ``(enc0, enc1, enc2)`` where ``enc0``,
``enc1``, and ``enc2`` refer to encoder values on axes
0, 1, and 2, respectively.
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:param Array[int, int] out:
An output buffer to store the wrist motor commands.
:raises TypeError:
If ``out`` does not support item assignment either
because it's immutable or not subscriptable.
:raises IndexError:
If ``len(out) < 3``.
:raises ctypes.ArgumentError:
If any element of ``t`` is not implicitly convertible to
a C double.
:raises IndexError:
If ``len(t) < 3``.
:raises TypeError:
If ``t`` is not subscriptable.
:raises ctypes.ArgumentError:
If any element of ``enc`` is not implicitly convertible
to a C int.
:raises IndexError:
If ``len(enc) < 3``.
:raises TypeError:
If ``enc`` is not subscriptable.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns:
0 on success, -1 otherwise
See Also
--------
| :func:`forcedimension_core.dhd.expert.wristTorqueToMotor()`
"""
output0 = c_ushort()
output1 = c_ushort()
output2 = c_ushort()
err = _runtime._libdhd.dhdWristTorqueToMotor(
t[0], t[1], t[2],
enc[0], enc[1], enc[2],
output0, output1, output2,
ID
)
out[0] = output0.value
out[1] = output1.value
out[2] = output2.value
return err
_runtime._libdhd.dhdGripperEncoderToAngleRad.argtypes = [
c_int,
c_double_ptr,
c_byte
]
_runtime._libdhd.dhdGripperEncoderToAngleRad.restype = c_int
[docs]def gripperEncoderToAngleRad(enc: int, out: c_double, ID: int = -1) -> int:
"""
Compute the gripper angle (in [rad]) for a given encoder value.
Note
----
This feature only applies to devices with a gripper. See the
:ref:`device_types` section for more details.
:param int enc:
Gripper encoder reading.
:param c_int out:
Output buffer to store the gripper angle (in [rad]).
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:raises ctypes.ArgumentError:
If ``enc`` is not implicitly convertible to a C char.
:raises IndexError:
If ``len(enc) < 3``
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns:
0 or :data:`forcedimension_core.constants.TIMEGUARD`
on success, -1 otherwise.
See Also
--------
| :func:`forcedimension_core.dhd.expert.gripperAngleRadToEncoder()`
| :func:`forcedimension_core.dhd.expert.gripperEncoderToGap()`
"""
return _runtime._libdhd.dhdGripperEncoderToAngleRad(enc, out, ID)
_runtime._libdhd.dhdGripperEncoderToGap.argtypes = [
c_int, c_double_ptr, c_byte
]
_runtime._libdhd.dhdGripperEncoderToGap.restype = c_int
[docs]def gripperEncoderToGap(enc: int, out: c_double, ID: int = -1) -> int:
"""
Compute and return the gripper opening (in [m]) for a
given encoder reading.
Note
----
This feature only applies to devices with a gripper. See
the :ref:`device_types` section for more details.
:param int enc:
Gripper encoder reading.
:param c_double out:
Buffer to store the griper opening (in [m]).
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:raises ctypes.ArgumentError:
If ``enc`` is not implicitly convertible to a C char.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns:
0 on success, -1 otherwise.
See Also
--------
| :func:`forcedimension_core.dhd.expert.gripperGapToEncoder()`
| :func:`forcedimension_core.dhd.expert.gripperEncoderToAngleRad()`
"""
return _runtime._libdhd.dhdGripperEncoderToGap(enc, out, ID)
_runtime._libdhd.dhdGripperAngleRadToEncoder.argtypes = [
c_double, c_int_ptr, c_byte
]
_runtime._libdhd.dhdGripperAngleRadToEncoder.restype = c_int
[docs]def gripperAngleRadToEncoder(angle: float, out: c_int, ID: int = -1) -> int:
"""
Computes and return the gripper encoder value for a given
gripper opening distance (in [rad]).
Note
----
This feature only applies to devices with a gripper. See
the :ref:`device_types` section for more details.
:param float angle:
Gripper opening as an angle (in [rad]).
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:raises ctypes.ArgumentError:
If ``angle`` is not implicitly convertible to a C char.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns:
0 on success, -1 otherwise.
See Also
--------
| :func:`forcedimension_core.dhd.expert.gripperEncoderToAngleRad()`
| :func:`forcedimension_core.dhd.expert.gripperEncoderToGap()`
"""
return _runtime._libdhd.dhdGripperAngleRadToEncoder(angle, out, ID)
_runtime._libdhd.dhdGripperGapToEncoder.argtypes = [
c_double, c_int_ptr, c_byte
]
_runtime._libdhd.dhdGripperGapToEncoder.restype = c_int
[docs]def gripperGapToEncoder(gap: float, out: c_int, ID: int = -1) -> int:
"""
Compute and return the gripper encoder value for a given
gripper opening distance (in [m]).
Note
----
This feature only applies to devices with a gripper. See
the :ref:`device_types` section for more details.
:param float gap:
Gripper opening distance (in [m]).
:param c_int out:
Outpu buffer to store the gripper encoder value.
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:raises ctypes.ArgumentError:
If ``gap`` is not implicitly convertible to a C char.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns:
0 on success, -1 otherwise.
See Also
--------
| :func:`forcedimension_core.dhd.expert.gripperEncoderToAngleRad()`
| :func:`forcedimension_core.dhd.expert.gripperAngleRadToEncoder()`
| :func:`forcedimension_core.dhd.expert.gripperEncoderToGap()`
"""
return _runtime._libdhd.dhdGripperGapToEncoder(gap, out, ID)
_runtime._libdhd.dhdGripperMotorToForce.argtypes = [
c_ushort, c_double_ptr, c_int_ptr, c_byte
]
_runtime._libdhd.dhdGripperMotorToForce.restype = c_int
[docs]def gripperMotorToForce(
cmd: int,
enc_wrist: Array[int, int],
enc_gripper: int,
out: c_double,
ID: int = -1
) -> int:
"""
Compute the force applied to the end-effector for a given
motor command.
Note
----
This feature only applies to devices with an active gripper.
See the :ref:`device_types` section for more details.
:param int output:
Motor command on gripper axis.
:param Array[int, int] enc_wrist:
Sequence of ``(enc0, enc1, enc2)`` where ``enc0``,
``enc1``, ``enc2`` are encoder values about wrist
joints 0, 1, and 2, respectively.
:param int enc_gripper:
Encoder reading for the gripper.
:param c_double out:
Output buffer to store the force applied to the end
effector (in [N]).
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:raises ctypes.ArgumentError:
If ``cmd`` is not implicitly convertible to a C ushort.
:raises ctypes.ArgumentError:
If any element of ``enc_wrist`` is not implicitly
convertible to a C int.
:raises IndexError:
If ``len(enc_wrist) < 3``.
:raises TypeError:
If ``enc_wrist`` is not subscriptable.
:raises ctypes.ArgumentError:
If ``enc_gripper`` is not implicitly convertible to
a C int.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns:
0 on success, -1 otherwise.
See Also
--------
| :func:`forcedimension_core.dhd.expert.gripperForceToMotor()`
"""
enc = (c_int * 4)(enc_wrist[0], enc_wrist[1], enc_wrist[2], enc_gripper)
return _runtime._libdhd.dhdGripperMotorToForce(cmd, out, enc, ID)
_runtime._libdhd.dhdGripperForceToMotor.argtypes = [
c_double, c_ushort_ptr, c_int_ptr, c_byte
]
_runtime._libdhd.dhdGripperForceToMotor.restype = c_int
[docs]def gripperForceToMotor(
f: float,
enc_wrist: Array[int, int],
enc_gripper: int,
out: c_ushort,
ID: int = -1
) -> int:
"""
Given a desired force (in [N]) to be displayed by the force
gripper, compute and return the refering motor command.
Note
----
This feature only applies to devices with an active gripper.
See the :ref:`device_types` section for more details.
:param int f:
Force on the gripper end-effector (in [N]).
:param Array[int, int] enc_wrist:
An output buffer to store the wrist encoding readings.
:param c_ushort out:
Output buffer to store the motor command.
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:raises ctypes.ArgumentError:
If ``f`` is not implicitly convertible to a C double.
:raises ctypes.ArgumentError:
If any member of ``enc_wrist`` is not implicitly
convertible to a C ushort.
:raises IndexError:
If ``len(enc_wrist) < 3``.
:raises TypeError:
If ``enc_wrist`` is not subscriptable.
:raises ctypes.ArgumentError:
If ``enc_gripper`` is not implicitly convertible to a C ushort.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns:
0 or :data:`forcedimension_core.constants.MOTOR_SATURATED`
on success, -1 otherwise.
See Also
--------
| :func:`forcedimension_core.dhd.expert.gripperForceToMotor()`
"""
enc = (c_int * 4)(enc_wrist[0], enc_wrist[1], enc_wrist[2], enc_gripper)
return _runtime._libdhd.dhdGripperForceToMotor(f, out, enc, ID)
_runtime._libdhd.dhdSetMot.argtypes = [c_ushort_ptr, c_ubyte, c_byte]
_runtime._libdhd.dhdSetMot.restype = c_int
[docs]def setMot(cmds: Array[int, int], mask: int = 0xff, ID: int = -1) -> int:
"""
Program motor commands to a selection of motor channels.
Particularly useful when using the generic controller
directly, without a device model attached.
:param Array[int, int] cmds:
List of motor command values.
:param int mask:
Bitwise mask of which motor should be set.
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:raises ctypes.ArgumentError:
If any element of ``cmds`` is not implicitly convertible
to a C ushort.
:raises IndexError:
If ``len(cmds) < MAX_DOF``.
:raises TypeError:
If ``cmds`` is not subscriptable.
:raises ctypes.ArgumentError:
If ``mask`` is not implicitly convertible to a C uchar.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns: 0 on success, -1 otherwise
See Also
--------
| :data:`forcedimension_core.dhd.expert.setMotor()`
| :data:`forcedimension_core.dhd.expert.setDeltaMotor()`
| :data:`forcedimension_core.dhd.expert.setWristMotor()`
| :data:`forcedimension_core.dhd.expert.setGripperMotor()`
"""
cmd_arr = (c_ushort * MAX_DOF)(
cmds[0],
cmds[1],
cmds[2],
cmds[3],
cmds[4],
cmds[5],
cmds[6],
cmds[7]
)
return _runtime._libdhd.dhdSetMot(
ct.cast(cmd_arr, c_ushort_ptr),
mask,
ID
)
_runtime._libdhd.dhdSetJointTorques.argtypes = [
c_double_ptr, c_ubyte, c_byte
]
_runtime._libdhd.dhdSetJointTorques.restype = c_int
[docs]def setJointTorques(q: Array[int, float], mask: int = 0xff, ID: int = -1):
"""
Sets all joint torques on all active axes.
:param Array[int, float] q:
Joint torques for each degree-of-freedom (in [Nm]).
:param int mask:
Bitwise mask of which joints should be set
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:raises ctypes.ArgumentError:
If any element of ``cmds`` is not implicitly convertible
to a C double.
:raises IndexError:
If ``len(q) < MAX_DOF``.
:raises TypeError:
If ``q`` is not subscriptable.
:raises ctypes.ArgumentError:
If ``mask`` is not implicitly convertible to a C uchar.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns:
0 on success, -1 otherwise.
See Also
--------
| :func:`forcedimension_core.dhd.expert.setDeltaJointTorques()`
| :func:`forcedimension_core.dhd.expert.setWristJointTorques()`
| :func:`forcedimension_core.dhd.expert.setForceAndWristJointTorques()`
| :func:`forcedimension_core.dhd.expert.setForceAndWristJointTorquesAndGripperForce()`
"""
cmd_arr = (c_double * MAX_DOF)(
q[0], q[1], q[2], q[3], q[4], q[5], q[6], q[7]
)
return _runtime._libdhd.dhdSetJointTorques(
ct.cast(cmd_arr, c_double_ptr), mask, ID
)
_runtime._libdhd.dhdPreloadMot.argtypes = [c_ushort_ptr, c_ubyte, c_byte]
_runtime._libdhd.dhdPreloadMot.restype = c_int
[docs]def preloadMot(cmds: Array[int, int], mask: int = 0xff, ID: int = -1) -> int:
"""
Program motor commands to a selection of motor channels.
Unlike :func:`forcedimension_core.dhd.expert.setMot()`, this
function saves the requested commands internally for later
application by calling
:func:`forcedimension_core.dhd.setForce()` and the friends.
:param Array[int, int] outputs:
List of motor command values.
:param int mask:
Bitwise mask of which motor should be set.
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:raises ctypes.ArgumentError:
If any element of ``cmds`` is not implicitly convertible
to a C ushort.
:raises IndexError:
If ``len(cmds) < MAX_DOF``.
:raises TypeError:
If ``cmds`` is not subscriptable.
:raises ctypes.ArgumentError:
If ``mask`` is not implicitly convertible to a C char.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns: 0 on success, -1 otherwise
See Also
--------
| :func:`forcedimension_core.dhd.expert.setMot()`
"""
cmd_arr = (c_ushort * MAX_DOF)(
cmds[0],
cmds[1],
cmds[2],
cmds[3],
cmds[4],
cmds[5],
cmds[6],
cmds[7]
)
return _runtime._libdhd.dhdPreloadMot(
ct.cast(cmd_arr, c_ushort_ptr),
mask,
ID
)
_runtime._libdhd.dhdGetEnc.argtypes = [c_int_ptr, c_ubyte, c_byte]
_runtime._libdhd.dhdGetEnc.restype = c_int
[docs]def getEnc(out: MutableArray[int, int], mask: int = 0xff, ID: int = -1) -> int:
"""
Get a selective list of encoder values. Particularly useful
when using the generic controller directly, without a device
model attached.
:param int mask:
Bitwise mask of which motor should be set.
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:param Array[int, int] out:
An output buffer to store the encoder values.
:raises TypeError:
If ``out`` does not support item assignment either
because it's immutable or not subscriptable.
:raises IndexError:
If ``len(out) < MAX_DOF``
:raises ctypes.ArgumentError:
If ``mask`` is not implicitly convertible to a C char.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns:
0 or :data:`forcedimension_core.constants.TIMEGUARD`
on success, -1 otherwise.
See Also
--------
| :data:`forcedimension_core.dhd.expert.getEncoder()`
| :data:`forcedimension_core.dhd.expert.getEncVelocities()`
"""
enc = (c_int * MAX_DOF)()
err = _runtime._libdhd.dhdGetEnc(ct.cast(enc, c_int_ptr), mask, ID)
for i in range(MAX_DOF):
out[i] = enc[i]
return err
_runtime._libdhd.dhdSetBrk.argtypes = [c_ubyte, c_byte]
_runtime._libdhd.dhdSetBrk.restype = c_int
[docs]def setBrk(mask: int = 0xff, ID: int = -1) -> int:
"""
Set electromagnetic braking status on selective motor groups.
Only applies when using the generic controller directly,
without a device model attached.
Generic control motor groups
group1 - [mot0, mot1, mot2]
group2 - [mot3, mot4, mot5]
group3 - [mot6]
group4 - [mot7]
The mask parameter addresses all 8 motors bitwise. If a
single bit within a motor group's is enabled, all motors in
that motor's group's electromagnetic brakes will be activated.
:param mask:
Bitwise mask of which motor groups should have their
electromagnetic brakes be set on.
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:raises ctypes.ArgumentError:
If ``mask`` is not implicitly convertible to a C char.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns: 0 on success, -1 otherwise
"""
return _runtime._libdhd.dhdSetBrk(mask, ID)
_runtime._libdhd.dhdGetDeltaJointAngles.argtypes = [
c_double_ptr, c_double_ptr, c_double_ptr, c_byte
]
_runtime._libdhd.dhdGetDeltaJointAngles.restype = c_int
[docs]def getDeltaJointAngles(out: MutableArray[int, float], ID: int = -1) -> int:
"""
Retrieve the joint angles (in [rad]) for the DELTA structure.
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:param MutableArray[int, float] out:
An output buffer to store the joint angles.
:raises TypeError:
If ``out`` does not support item assignment either
because it's immutable or not subscriptable.
:raises IndexError:
If ``len(out) < 3``.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns:
0 or :data:`forcedimension_core.constants.TIMEGUARD`
on success, -1 otherwise.
See Also
--------
| :func:`forcedimension_core.dhd.expert.deltaEncodersToJointAngles()`
| :func:`forcedimension_core.dhd.expert.deltaJointAnglesToEncoders()`
| :func:`forcedimension_core.dhd.expert.deltaJointAnglesToJacobian()`
"""
j0 = c_double()
j1 = c_double()
j2 = c_double()
err = _runtime._libdhd.dhdGetDeltaJointAngles(
j0, j1, j2, ID
)
out[0] = j0.value
out[1] = j1.value
out[2] = j2.value
return err
_runtime._libdhd.dhdGetDeltaJacobian.argtypes = [c_double_ptr, c_byte]
_runtime._libdhd.dhdGetDeltaJacobian.restype = c_int
[docs]def getDeltaJacobian(
out: Array[int, MutableArray[int, float]],
ID: int = -1
) -> int:
"""
Retrieve the 3x3 jacobian matrix for the DELTA structure
based on the current end-effector position. Please refer to
your device user manual for more information on your device
coordinate system.
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:param MutableFloatMatrixLike out:
An output buffer to store the Jacobian.
:raises TypeError:
If ``out`` does not support item assignment either
because it's immutable or not subscriptable.
:raises IndexError:
If any dimension of ``out`` is less than 3.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns:
0 or :data:`forcedimension_core.constants.TIMEGUARD`
on success, -1 otherwise.
See Also
--------
| :func:`forcedimension_core.dhd.expert.deltaJointAnglesToJacobian()`
"""
J = ((c_double * 3) * 3)()
err = _runtime._libdhd.dhdGetDeltaJacobian(ct.cast(J, c_double_ptr), ID)
for i in range(3):
for j in range(3):
out[i][j] = J[i][j]
return err
_runtime._libdhd.dhdDeltaJointAnglesToJacobian.argtypes = [
c_double, c_double, c_double,
c_double_ptr,
c_byte
]
_runtime._libdhd.dhdDeltaJointAnglesToJacobian.restype = c_int
[docs]def deltaJointAnglesToJacobian(
joint_angles: Array[int, float],
out: Array[int, MutableArray[int, float]],
ID: int = -1,
) -> int:
"""
Retrieve the 3x3 jacobian matrix for the DELTA structure
based on a given joint configuration. Please refer to your
device user manual for more information on your device
coordinate system.
:param Array[int, float] joint_angles:
Sequence of (j0, j1, j2) where ``j0``, ``j1``, and ``j2``
refer to the joint angles for axes 0, 1, and 2,
respectively.
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:param MutableFloatMatrixLike out:
An output buffer to store the return.
:raises TypeError:
If ``out`` does not support item assignment either
because it's immutable or not subscriptable.
:raises IndexError:
If ``len(out) < 3``.
:raises ctypes.ArgumentError:
If any element of ``joint_angles`` is not implicitly
convertible to C double.
:raises IndexError:
If ``len(joint_angles) < 3``.
:raises TypeError:
If ``joint_angles`` is not subscriptable.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns:
0 on success, -1 otherwise.
See Also
--------
| :func:`forcedimension_core.dhd.expert.getJointAngles()`
| :func:`forcedimension_core.dhd.expert.getDeltaJointAngles()`
| :func:`forcedimension_core.dhd.expert.getDeltaJacobian()`
"""
J = ((c_double * 3) * 3)()
err = _runtime._libdhd.dhdDeltaJointAnglesToJacobian(
joint_angles[0], joint_angles[1], joint_angles[2],
ct.cast(J, c_double_ptr),
ID
)
for i in range(3):
for j in range(3):
out[i][j] = J[i][j]
return err
_runtime._libdhd.dhdDeltaJointTorquesExtrema.argtypes = [
c_double, c_double, c_double,
c_double_ptr, c_double_ptr,
c_byte
]
_runtime._libdhd.dhdDeltaJointTorquesExtrema.restype = c_int
[docs]def deltaJointTorquesExtrema(
joint_angles: Array[int, float],
minq_out: MutableArray[int, float],
maxq_out: MutableArray[int, float],
ID: int = -1
) -> int:
"""
Compute the range of applicable DELTA joint torques for a
given DELTA joint angle configuration. Please refer to your
device user manual for more information on your device
coordinate system.
:param Array[int, float] joint_angles:
Sequence of ``(j0, j1, j2)`` where ``j0``, ``j1``, ``j2``
refer to the joint angles for axes 0, 1, and 2,
respectively.
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:param MutableArray[int, float] minq_out:
An output buffer to store the return.
:param MutableArray[int, float] maxq_out:
An output buffer to store the return.
:raises TypeError:
If ``minq_out`` does not support item assignment
either because it's immutable or not subscriptable.
:raises IndexError:
If ``len(encMin_out) < MAX_DOF``
:raises TypeError:
If ``maxq_out`` does not support item assignment
either because it's immutable or not subscriptable.
:raises IndexError:
If ``len(out) < MAX_DOF``
:raises ctypes.ArgumentError:
If any element of ``joint_angles`` is not implicitly
convertible to a C double.
:raises IndexError:
If ``len(joint_angles) < 3``.
:raises TypeError:
If ``joint_angles`` is not subscriptable.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns:
0 on success, -1 otherwise.
See Also
--------
| :func:`forcedimension_core.dhd.expert.wristJointTorquesExtrema()`
| :func:`forcedimension_core.dhd.expert.getDeltaJointAngles()`
| :func:`forcedimension_core.dhd.expert.getJointAngles()`
"""
minq = (c_double * 3)()
maxq = (c_double * 3)()
err = _runtime._libdhd.dhdDeltaJointTorquesExtrema(
joint_angles[0], joint_angles[1], joint_angles[2],
ct.cast(minq, c_double_ptr), ct.cast(maxq, c_double_ptr),
ID
)
minq_out[0] = minq[0]
minq_out[1] = minq[1]
minq_out[2] = minq[2]
maxq_out[0] = maxq[0]
maxq_out[1] = maxq[1]
maxq_out[2] = maxq[2]
return err
_runtime._libdhd.dhdSetDeltaJointTorques.argtypes = [
c_double, c_double, c_double, c_byte
]
_runtime._libdhd.dhdSetDeltaJointTorques.restype = c_int
[docs]def setDeltaJointTorques(
q: Array[int, float],
ID: int = -1
) -> int:
"""
Set all joint torques of the DELTA structure.
:param Array[int, float] t:
Sequence of ``(t0, t1, t2)`` where ``t0``, ``t1``, and
``t2`` are the DELTA axis torque commands for axes 0, 1,
and 2, respectively (in [Nm]).
:raises ctypes.ArgumentError:
If any element of ``t`` is not implicitly convertible
to a C char.
:raises IndexError:
If ``len(t) < 3``.
:raises TypeError:
If ``t`` is not subscriptable.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns: 0 on success, -1 otherwise
See Also
--------
| :func:`forcedimension_core.dhd.expert.deltaJointTorquesExtrema()`
| :func:`forcedimension_core.dhd.expert.setWristJointTorques()`
| :func:`forcedimension_core.dhd.expert.setForceAndWristJointTorques()`
| :func:`forcedimension_core.dhd.expert.setForceAndWristJointTorquesAndGripperForce()`
"""
return _runtime._libdhd.dhdSetDeltaJointTorques(q[0], q[1], q[2], ID)
_runtime._libdhd.dhdDeltaEncodersToJointAngles.argtypes = [
c_int, c_int, c_int,
c_double_ptr, c_double_ptr, c_double_ptr,
c_byte
]
_runtime._libdhd.dhdDeltaEncodersToJointAngles.restype = c_int
[docs]def deltaEncodersToJointAngles(
enc: Array[int, int],
out: MutableArray[int, float],
ID: int = -1
) -> int:
"""
Compute and return the DELTA joint angles for a given set of
encoder values.
:param Array[int, int] enc:
Sequence of ``(enc0, enc1, enc2)`` where ``enc0``,
``enc1``, and ``enc2`` refer to encoder values on axes
0, 1, and 2, respectively.
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:param MutableArray[int, float] out:
An output buffer to store the return.
:raises TypeError:
If ``out`` does not support item
assignment either because it's immutable or not subscriptable.
:raises IndexError:
If ``len(out) < 3``.
:raises ctypes.ArgumentError:
If any element of ``enc`` is not implicitly convertible
to a C int.
:raises IndexError:
If ``len(enc) < 3``.
:raises TypeError:
If ``enc`` is not subscriptable.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns:
0 on success, -1 otherwise.
See Also
--------
| :func:`forcedimension_core.dhd.expert.deltaJointAnglesToJacobian()`
| :func:`forcedimension_core.dhd.expert.deltaJointAnglesToEncoders()`
"""
j0 = c_double()
j1 = c_double()
j2 = c_double()
err = _runtime._libdhd.dhdDeltaEncodersToJointAngles(
enc[0], enc[1], enc[2],
j0, j1, j2,
ID
)
out[0] = j0.value
out[1] = j1.value
out[2] = j2.value
return err
_runtime._libdhd.dhdDeltaJointAnglesToEncoders.argtypes = [
c_double, c_double, c_double,
c_int_ptr, c_int_ptr, c_int_ptr,
c_byte
]
_runtime._libdhd.dhdDeltaJointAnglesToEncoders.restype = c_int
[docs]def deltaJointAnglesToEncoders(
joint_angles: Array[int, float],
out: MutableArray[int, int],
ID: int = -1,
) -> int:
"""
Compute and return the DELTA encoder values for a given
set of joint angles.
:param Array[int, float] enc:
Sequence of ``(j0, j1, j1)`` where ``j0``, ``j1``, and
``j2`` refer to DELTA joint angles for axes 0, 1, and 2,
respectively, (in [rad]).
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:param MutableArray[int, float] out:
An output buffer to store the return.
:raises TypeError:
If ``out`` does not support item
assignment either because it's immutable or not
subscriptable.
:raises IndexError:
If ``len(out) < 3``.
:raises ctypes.ArgumentError:
If any element of ``joint_angles`` is not implicitly
convertible to a C double.
:raises IndexError:
If ``len(joint_angles) < 3``.
:raises TypeError:
If ``joint_angles`` is not subscriptable.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns:
0 on success, -1 otherwise.
See Also
--------
| :func:`forcedimension_core.dhd.expert.deltaJointAnglesToJacobian()`
| :func:`forcedimension_core.dhd.expert.deltaEncodersToJointAngles()`
"""
enc0 = c_int()
enc1 = c_int()
enc2 = c_int()
err = _runtime._libdhd.dhdDeltaJointAnglesToEncoders(
joint_angles[0], joint_angles[1], joint_angles[2],
enc0, enc1, enc2,
ID
)
out[0] = enc0.value
out[1] = enc1.value
out[2] = enc2.value
return err
_runtime._libdhd.dhdGetWristJointAngles.argtypes = [
c_double_ptr, c_double_ptr, c_double_ptr,
c_byte
]
_runtime._libdhd.dhdGetWristJointAngles.restype = c_int
[docs]def getWristJointAngles(out: MutableArray[int, float], ID: int = -1) -> int:
"""
Retrieve the joint angles (in [rad]) for the wrist structure.
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:param MutableArray[int, float] out:
An output buffer to store the wrist joint angles.
:raises TypeError:
If ``out`` does not support item assignment either
because it's immutable or not subscriptable.
:raises IndexError:
If ``len(out) < 3``.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns:
0 or :data:`forcedimension_core.constants.TIMEGUARD`
on success, -1 otherwise.
See Also
--------
| :func:`forcedimension_core.dhd.expert.dhdWristJointAnglesToJacobian()`
| :func:`forcedimension_core.dhd.expert.dhdWristJointTorquesExtrema()`
| :func:`forcedimension_core.dhd.expert.wristJointAnglesToEncoders()`
| :func:`forcedimension_core.dhd.expert.wristEncodersToJointAngles()`
"""
j0 = c_double()
j1 = c_double()
j2 = c_double()
err = _runtime._libdhd.dhdGetWristJointAngles(
j0, j1, j2, ID
)
out[0] = j0.value
out[1] = j1.value
out[2] = j2.value
return err
_runtime._libdhd.dhdGetWristJacobian.argtypes = [c_double_ptr, c_byte]
_runtime._libdhd.dhdGetWristJacobian.restype = c_int
[docs]def getWristJacobian(
out: Array[int, MutableArray[int, float]],
ID: int = -1
) -> int:
"""
Retrieve the 3x3 jacobian matrix for the wrist structure
based on the current end-effector position. Please refer to
your device user manual for more information on your device
coordinate system.
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:param MutableArray[int, float] out:
An output buffer to store the Jacobian.
:raises TypeError:
If ``out`` does not support item assignment either
because it's immutable or not subscriptable.
:raises IndexError:
If any dimension of out is less than 3.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns:
0 or :data:`forcedimension_core.constants.TIMEGUARD`
on success, and -1 otherwise.
See Also
--------
| :func:`forcedimension_core.dhd.expert.wristJointAnglesToJacobian()`
"""
J = ((c_double * 3) * 3)()
err = _runtime._libdhd.dhdGetWristJacobian(ct.cast(J, c_double_ptr), ID)
for i in range(3):
for j in range(3):
out[i][j] = J[i][j]
return err
_runtime._libdhd.dhdWristJointAnglesToJacobian.argtypes = [
c_double, c_double, c_double,
c_double_ptr,
c_byte
]
_runtime._libdhd.dhdWristJointAnglesToJacobian.restype = c_int
[docs]def wristJointAnglesToJacobian(
joint_angles: Array[int, float],
out: Array[int, MutableArray[int, float]],
ID: int = -1,
) -> int:
"""
Retrieve the 3x3 jacobian matrix for the wrist structure
based on a given joint configuration. Please refer to your
device user manual for more information on your device
coordinate system.
:param Array[int, float] joint_angles:
Sequence of ``(j0, j1, j2)`` where ``j0``, ``j1``, and
``j2`` refer to the joint angles for wrist axis 0, 1,
and 2, respectively.
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:param MutableFloatMatrixLike out:
An output buffer to store the Jacobian.
:raises TypeError:
If ``out`` does not support item assignment either
because it's immutable or not subscriptable.
:raises IndexError:
If ``len(out) < 3``.
:raises ctypes.ArgumentError:
If any element of ``joint_angles`` is not implicitly
convertible to a C double.
:raises IndexError:
If ``len(joint_angles) < 3``.
:raises TypeError:
If ``joint_angles`` is not subscriptable.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns:
0 on success, -1 otherwise.
See Also
--------
| :func:`forcedimension_core.dhd.wristEncodersToJointAngles()`
| :func:`forcedimension_core.dhd.getWristJointAngles()`
"""
J = ((c_double * 3) * 3)()
err = _runtime._libdhd.dhdWristJointAnglesToJacobian(
joint_angles[0], joint_angles[1], joint_angles[2],
ct.cast(J, c_double_ptr),
ID
)
for i in range(3):
for j in range(3):
out[i][j] = J[i][j]
return err
_runtime._libdhd.dhdWristJointTorquesExtrema.argtypes = [
c_double, c_double, c_double,
c_double_ptr, c_double_ptr,
c_byte
]
_runtime._libdhd.dhdWristJointTorquesExtrema.restype = c_int
[docs]def wristJointTorquesExtrema(
joint_angles: Array[int, float],
minq_out: MutableArray[int, float],
maxq_out: MutableArray[int, float],
ID: int = -1
) -> int:
"""
Compute the range of applicable wrist joint torques for a
given wrist joint angle configuration. Please refer to your
device user manual for more information on your device
coordinate system.
:param Array[int, float] joint_angles:
Sequence of ``(j0, j1, j2)`` where ``j0``, ``j1``, ``j2``
refer to the joint angles for wrist axes 0, 1, and 2,
respectively.
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:param MutableArray[int, float] minq_out:
An output buffer to store the return.
:param MutableArray[int, float] maxq_out:
An output buffer to store the return.
:raises TypeError:
If ``minq_out`` does not support item assignment
either because it's immutable or not subscriptable.
:raises IndexError:
If ``len(minq_out) < MAX_DOF``
:raises TypeError:
If ``maxq_out`` does not support item assignment
either because it's immutable or not subscriptable.
:raises IndexError:
If ``len(maxq_out) < MAX_DOF``
:raises ctypes.ArgumentError:
If any element of ``joint_angles`` is not implicitly
convertible to C double.
:raises IndexError:
If ``len(joint_angles) < 3``.
:raises TypeError:
If ``joint_angles`` is not subscriptable.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns:
0 on success, -1 otherwise.
See Also
--------
| :func:`forcedimension_core.dhd.expert.getJointAngles()`
| :func:`forcedimension_core.dhd.expert.getWristJointAngles()`
| :func:`forcedimension_core.dhd.expert.wristEncodersToJointAngles()`
| :func:`forcedimension_core.dhd.expert.wristJointAnglesToJacobian()`
| :func:`forcedimension_core.dhd.expert.wristJointAnglesToEncoders()`
"""
minq = (c_double * 3)()
maxq = (c_double * 3)()
err = _runtime._libdhd.dhdWristJointTorquesExtrema(
joint_angles[0], joint_angles[1], joint_angles[2],
ct.cast(minq, c_double_ptr), ct.cast(maxq, c_double_ptr),
ID
)
minq_out[0] = minq[0]
minq_out[1] = minq[1]
minq_out[2] = minq[2]
maxq_out[0] = maxq[0]
maxq_out[1] = maxq[1]
maxq_out[2] = maxq[2]
return err
_runtime._libdhd.dhdSetWristJointTorques.argtypes = [
c_double, c_double, c_double, c_byte
]
_runtime._libdhd.dhdSetWristJointTorques.restype = c_int
[docs]def setWristJointTorques(
t: Array[int, float],
ID: int = -1
) -> int:
"""
Set all joint torques of the wrist structure.
:param Array[int, float] t:
Sequence of (t0, t1, t2) where t0, t1, t2 are the wrist
axis torque commands for axes 0, 1, and 2, respectively
(in [Nm]).
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:raises ctypes.ArgumentError:
If any element of ``t`` is not implicitly convertible
to a C double.
:raises IndexError:
If ``len(t) < 3``.
:raises TypeError:
If ``t`` is not subscriptable.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns: 0 on success, -1 otherwise.
See Also
--------
| :func:`forcedimension_core.dhd.expert.setForceAndWristJointTorques()`
| :func:`forcedimension_core.dhd.expert.setForceAndWristJointTorquesAndGripperForce()`
"""
return _runtime._libdhd.dhdSetWristJointTorques(t[0], t[1], t[2], ID)
_runtime._libdhd.dhdSetForceAndWristJointTorques.argtypes = [
c_double, c_double, c_double,
c_double, c_double, c_double,
c_byte
]
_runtime._libdhd.dhdSetForceAndWristJointTorques.restype = c_int
[docs]def setForceAndWristJointTorques(
f: Array[int, float],
t: Array[int, float],
ID: int = -1
) -> int:
"""
Set force (in [N]) and wrist joint torques (in [Nm]) about
the X, Y, and Z axes.
:param Array[int, float] f:
Sequence of ``(fx, fy, fz)`` where ``fx``, ``fy``, and
``fz`` are the translation forces (in [N]) to be applied
to the DELTA end-effector on the X, Y, and Z axes
respectively.
:param Array[int, float] t:
Sequence of (t0, t1, t2) where ``t0``, ``t1``, ``t2``
are the wrist joint torques (in [Nm]) to be applied to
the wrist end-effector for axes 0, 1, and 2,
respectively.
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:raises ctypes.ArgumentError:
If any element of ``f`` is not implicitly convertible
to a C double.
:raises IndexError:
If ``len(f) < 3``.
:raises TypeError:
If ``f`` is not subscriptable.
:raises ctypes.ArgumentError:
If any element of ``t`` is not implicitly convertible
to a C double.
:raises IndexError:
If ``len(t) < 3``.
:raises TypeError:
If ``t`` is not subscriptable.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns: 0 on success, -1 otherwise
See Also
--------
| :func:`forcedimension_core.dhd.expert.setWristJointTorques()`
| :func:`forcedimension_core.dhd.expert.setForceAndWristJointTorquesAndGripperForce()`
"""
return _runtime._libdhd.dhdSetForceAndWristJointTorques(
f[0], f[1], f[2], t[0], t[1], t[2], ID
)
_runtime._libdhd.dhdSetForceAndWristJointTorquesAndGripperForce.argtypes = [
c_double, c_double, c_double,
c_double, c_double, c_double,
c_double,
c_byte
]
_runtime._libdhd.dhdSetForceAndWristJointTorquesAndGripperForce.restype = c_int
[docs]def setForceAndWristJointTorquesAndGripperForce(
f: Array[int, float],
t: Array[int, float],
fg: float,
ID: int = -1) -> int:
"""
Set force (in [N]) and wrist joint torques (in [Nm]) about
the X, Y, and Z axes as well as the and gripper force.
:param Array[int, float] f:
Sequence of ``(fx, fy, fz)`` where ``fx``, ``fy``, and
``fz`` are the translation forces (in [N]) to be applied
to the DELTA end-effector on the X, Y, and Z axes
respectively.
:param Array[int, float] t:
Sequence of (t0, t1, t2) where ``t0``, ``t1``, ``t2``
are the wrist joint torques (in [Nm]) to be applied to
the wrist end-effector for axes 0, 1, and 2,
respectively.
:param float fg:
Gripper force (in [N]).
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:raises ctypes.ArgumentError:
If any element of ``f`` is not implicitly convertible to
a C double.
:raises IndexError:
If ``len(f) < 3``.
:raises TypeError:
If ``f`` is not subscriptable.
:raises ctypes.ArgumentError:
If any element of ``t`` is not implicitly convertible to
a C double.
:raises IndexError:
If ``len(t) < 3``.
:raises TypeError:
If ``t`` is not subscriptable.
:raises ctypes.ArgumentError:
If ``gripper_force`` is not implicitly convertible to
a C double.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns: 0 on success, -1 otherwise
See Also
--------
| :func:`forcedimension_core.dhd.expert.setWristJointTorques()`
| :func:`forcedimension_core.dhd.expert.setForceAndWristJointTorques()`
"""
return _runtime._libdhd.dhdSetForceAndWristJointTorquesAndGripperForce(
f[0], f[1], f[2], t[0], t[1], t[2], fg, ID
)
_runtime._libdhd.dhdWristEncodersToJointAngles.argtypes = [
c_int, c_int, c_int,
c_double_ptr, c_double_ptr, c_double_ptr,
c_byte
]
_runtime._libdhd.dhdWristEncodersToJointAngles.restype = c_int
[docs]def wristEncodersToJointAngles(
enc: Array[int, int],
out: MutableArray[int, float],
ID: int = -1
) -> int:
"""
Compute and return the wrist joint angles (in [rad]) for a
given set of encoder values.
:param Array[int, int] enc:
Sequence of (enc0, enc1, enc2) where ``enc0``, ``enc1``,
and ``enc2`` refer to encoder values on wrist axes
0, 1, and 2, respectively.
:param MutableArray[int, float] out:
An output buffer to store the joint angles (in [rad]).
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:raises ctypes.ArgumentError:
If any element of ``enc`` is not implicitly convertible
to a C int.
:raises IndexError:
If ``len(enc) < 3``.
:raises TypeError:
If ``enc`` is not subscriptable.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns:
0 on success, -1 otherwise.
See Also
--------
| :func:`forcedimension_core.dhd.expert.wristJointAnglesToJacobian()`
| :func:`forcedimension_core.dhd.expert.wristJointTorquesExtrema()`
| :func:`forcedimension_core.dhd.expert.wristJointAnglesToEncoders()`
"""
j0 = c_double()
j1 = c_double()
j2 = c_double()
err = _runtime._libdhd.dhdWristEncodersToJointAngles(
enc[0], enc[1], enc[2],
j0, j1, j2,
ID
)
out[0] = j0.value
out[1] = j1.value
out[2] = j2.value
return err
_runtime._libdhd.dhdWristJointAnglesToEncoders.argtypes = [
c_double, c_double, c_double,
c_int_ptr, c_int_ptr, c_int_ptr,
c_byte
]
_runtime._libdhd.dhdWristJointAnglesToEncoders.restype = c_int
[docs]def wristJointAnglesToEncoders(
joint_angles: Array[int, float],
out: MutableArray[int, int],
ID: int = -1
) -> int:
"""
Compute and return the wrist encoder values for a given set
of wrist joint angles (in [rad]).
:param Array[int, float] enc:
Sequence of ``(j0, j1, j1)`` where ``j0``, ``j1``, and
``j2`` refer to wrist joint angles for axes 0, 1, and
2, respectively, (in [rad]).
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:param Array[int, int] out:
An output buffer to store the raw encoder values.
:raises TypeError:
If ``out`` does not support item assignment either
because it's immutable or not subscriptable.
:raises IndexError:
If ``len(out) < 3``.
:raises ctypes.ArgumentError:
If any element of ``joint_angles`` is not implicitly
convertible to a C double.
:raises IndexError:
If ``len(joint_angles) < 3``.
:raises TypeError:
If ``joint_angles`` is not subscriptable.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns:
0 on success, -1 otherwise.
See Also
--------
| :func:`forcedimension_core.dhd.expert.wristJointAnglesToJacobian()`
| :func:`forcedimension_core.dhd.expert.wristEncodersToJointAngles()`
"""
enc0 = c_int()
enc1 = c_int()
enc2 = c_int()
err = _runtime._libdhd.dhdWristJointAnglesToEncoders(
joint_angles[0], joint_angles[1], joint_angles[2],
enc0, enc1, enc2,
ID
)
out[0] = enc0.value
out[1] = enc1.value
out[2] = enc2.value
return err
_runtime._libdhd.dhdGetJointAngles.argtypes = [c_double_ptr, c_byte]
_runtime._libdhd.dhdGetJointAngles.restype = c_int
[docs]def getJointAngles(out: MutableArray[int, float], ID: int = -1) -> int:
"""
Retrieve the joint angles (in [rad]) for all sensed
degrees-of-freedom of the current device.
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:param MutableArray[int, float] out:
An output buffer to store the joint angles (in [rad]).
:raises TypeError:
If ``out`` does not support item assignment either
because it's immutable or not subscriptable.
:raises IndexError:
If ``len(out) < MAX_DOF``
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns:
0 or :data:`forcedimension_core.constants.TIMEGUARD`
on success, -1 otherwise.
See Also
---------
| :func:`forcedimension_core.dhd.expert.getDeltaJointAngles()`
| :func:`forcedimension_core.dhd.expert.getWristJointAngles()`
| :func:`forcedimension_core.dhd.expert.deltaJointAnglesToEncoders()`
| :func:`forcedimension_core.dhd.expert.wristJointAnglesToEncoders()`
| :func:`forcedimension_core.dhd.expert.deltaJointTorquesExtrema()`
| :func:`forcedimension_core.dhd.expert.wristJointTorquesExtrema()`
| :func:`forcedimension_core.dhd.expert.deltaJointAnglesToJacobian()`
| :func:`forcedimension_core.dhd.expert.wristJointAnglesToJacobian()`
| :func:`forcedimension_core.dhd.expert.jointAnglesToIntertiaMatrix()`
| :func:`forcedimension_core.dhd.expert.jointAnglesToGravityJointTorques()`
| :func:`forcedimension_core.dhd.expert.getJointVelocities()`
"""
joint_angles = (c_double * MAX_DOF)()
err = _runtime._libdhd.dhdGetJointAngles(
ct.cast(joint_angles, c_double_ptr), ID)
for i in range(MAX_DOF):
out[i] = joint_angles[i]
return err
_runtime._libdhd.dhdGetJointVelocities.argtypes = [c_double_ptr, c_byte]
_runtime._libdhd.dhdGetJointVelocities.restype = c_int
[docs]def getJointVelocities(out: MutableArray[int, float], ID: int = -1) -> int:
"""
Retrieve the joint angle velocities (in [rad/s]) for all
sensed degrees-of-freedom of the current device.
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:param MutableArray[int, float] out:
An output buffer to store the joint velocities (in [rad/s]).
:raises TypeError:
If ``out`` does not support item assignment either
because it's immutable or not subscriptable.
:raises IndexError:
If ``len(out) < MAX_DOF``
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns:
0 or :data:`forcedimension_core.constants.TIMEGUARD`
on success, -1 otherwise.
See Also
--------
| :func:`forcedimension_core.dhd.expert.getJointAngles()`
| :func:`forcedimension_core.dhd.expert.getDeltaJacobian()`
| :func:`forcedimension_core.dhd.expert.getWristJacobian()`
"""
w = (c_double * MAX_DOF)()
err = _runtime._libdhd.dhdGetJointVelocities(ct.cast(w, c_double_ptr), ID)
for i in range(MAX_DOF):
out[i] = w[i]
return err
_runtime._libdhd.dhdGetEncVelocities.argtypes = [c_double_ptr, c_byte]
_runtime._libdhd.dhdGetEncVelocities.restype = c_int
[docs]def getEncVelocities(out: MutableArray[int, float], ID: int = -1) -> int:
"""
Retrieve the encoder angle velocities (in [inc/s]) for all
sensed degrees-of-freedom of the current device.
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:param MutableArray[int, float] out:
An output buffer to store the encoder velocities (in
[inc/s]).
:raises TypeError:
If ``out`` does not support item assignment either
because it's immutable or not subscriptable.
:raises IndexError:
If ``len(out) < MAX_DOF``
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns:
0 or :data:`forcedimension_core.constants.TIMEGUARD`
on success, -1 otherwise.
See Also
--------
| :data:`forcedimension_core.dhd.expert.getEnc()`
"""
v = (c_double * MAX_DOF)()
err = _runtime._libdhd.dhdGetEncVelocities(ct.cast(v, c_double_ptr), ID)
for i in range(MAX_DOF):
out[i] = v[i]
return err
_runtime._libdhd.dhdJointAnglesToInertiaMatrix.argtypes = [
c_double_ptr,
c_double_ptr,
c_byte
]
_runtime._libdhd.dhdJointAnglesToInertiaMatrix.restype = c_int
[docs]def jointAnglesToIntertiaMatrix(
joint_angles: Array[int, float],
out: Array[int, MutableArray[int, float]],
ID: int = -1,
) -> int:
"""
Retrieve the inertia matrix based on a given joint
configuration. Please refer to your device user manual for
more information on your device coordinate system.
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:param MutableFloatMatrixLike out:
Output buffer for the inertia matrix.
:raises ctypes.ArgumentError:
If ``joint_angles`` is not implicitly convertible to a
C char.
:raises IndexError:
If ``len(joint_angles) < 3``.
:raises TypeError:
If ``joint_angles`` is not subscriptable.
:raises TypeError:
If ``out`` does not support item assignment either
because it's immutable or not subscriptable.
:raises IndexError:
If of the any dimension of ``out`` is less than 6.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns:
0 on success, -1 otherwise.
See Also
--------
| :func:`forcedimension_core.dhd.expert.getJointAngles()`
"""
inertia = ((c_double * 6) * 6)()
joint_angles_arr = (c_double * MAX_DOF)(
joint_angles[0],
joint_angles[1],
joint_angles[2],
joint_angles[3],
joint_angles[4],
joint_angles[5],
joint_angles[6],
joint_angles[7]
)
err = _runtime._libdhd.dhdJointAnglesToInertiaMatrix(
ct.cast(joint_angles_arr, c_double_ptr),
ct.cast(inertia, c_double_ptr),
ID
)
for i in range(6):
for j in range(6):
out[i][j] = inertia[i][j]
return err
_runtime._libdhd.dhdJointAnglesToGravityJointTorques.argtypes = [
c_double_ptr,
c_double_ptr,
c_ubyte,
c_byte
]
_runtime._libdhd.dhdJointAnglesToGravityJointTorques.restype = c_int
[docs]def jointAnglesToGravityJointTorques(
joint_angles: Array[int, float],
out: MutableArray[int, float],
mask: int = 0xff,
ID: int = -1,
) -> int:
"""
This function computes the joint torques (in [Nm]) required
to provide gravity compensation on all actuated
degrees-of-freedom of the current device for a given joint
angles configuration (in [rad]).
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:param int mask:
Bitwise mask of which joint torques should be computed.
:param MutableFloatMatrixLike out:
Output buffer for the joint torques required to provide
gravity compensation (in [Nm]).
:raises ctypes.ArgumentError:
If ``joint_angles`` is not implicitly convertible to a
C char.
:raises IndexError:
If ``len(joint_angles) < forcedimension_core.constants.MAX_DOF``.
:raises TypeError:
If ``joint_angles`` is not subscriptable.
:raises TypeError:
If ``out`` does not support item assignment either
because it's immutable or not subscriptable.
:raises IndexError:
If ``len(out)`` is less than
:data:`forcedimension_core.constants.MAX_DOF`.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:raises ctypes.ArgumentError:
If ``mask`` is not implicitly convertible to a C uchar.
:returns:
0 on success, -1 otherwise.
See Also
--------
| :func:`forcedimension_core.dhd.expert.getJointAngles()`
"""
q = (c_double * MAX_DOF)()
joint_angles_arr = (c_double * MAX_DOF)(
joint_angles[0],
joint_angles[1],
joint_angles[2],
joint_angles[3],
joint_angles[4],
joint_angles[5],
joint_angles[6],
joint_angles[7]
)
err = _runtime._libdhd.dhdJointAnglesToGravityJointTorques(
ct.cast(joint_angles_arr, c_double_ptr),
ct.cast(q, c_double_ptr),
mask,
ID
)
for i in range(MAX_DOF):
out[i] = q[i]
return err
_runtime._libdhd.dhdSetComMode.argtypes = [c_int, c_byte]
_runtime._libdhd.dhdSetComMode.restype = c_int
[docs]def setComMode(mode: ComMode, ID: int = -1) -> int:
"""
Set the COM operation mode on compatible devices.
:param ComMode mode:
desired COM operation mode.
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:raises ctypes.ArgumentError:
If ``mode`` is not implicitly convertible to a C char.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns: 0 on success, -1 otherwise.
See Also
--------
| :func:`forcedimension_core.dhd.expert.setComModePriority`
"""
return _runtime._libdhd.dhdSetComMode(mode, ID)
_runtime._libdhd.dhdSetWatchdog.argtypes = [c_ubyte, c_byte]
_runtime._libdhd.dhdSetWatchdog.restype = c_int
[docs]def setWatchdog(duration: int, ID: int = -1) -> int:
"""
Set the watchdog duration in multiples of 125 microseconds
on compatible devices. If the watchdog duration is exceeded
before the device recieves a new force command, the device
firmware will disable forces.
:param int duration:
watchdog duration in multiples of 125 us.
A value of 0 disables the watchdog feature.
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:raises ctypes.ArgumentError:
If ``duration`` is not implicitly convertible to a C char.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns: 0 on success, -1 otherwise.
See Also
--------
| :func:`forcedimension_core.dhd.expert.getWatchdog()`
"""
return _runtime._libdhd.dhdSetWatchdog(duration, ID)
_runtime._libdhd.dhdGetWatchdog.argtypes = [c_ubyte_ptr, c_byte]
_runtime._libdhd.dhdGetWatchdog.restype = c_int
[docs]def getWatchdog(ID: int = -1) -> int:
"""
Get the watchdog duration in multiples of 125 us on
compatible devices.
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns:
The (non-negative) watchdog duration on success, -1 otherwise.
See Also
--------
| :func:`forcedimension_core.dhd.expert.setWatchdog()`
"""
duration = c_ubyte()
if _runtime._libdhd.dhdGetWatchdog(duration, ID):
return -1
return duration.value
_runtime._libdhd.dhdGetEncRange.argtypes = [c_int_ptr, c_int_ptr, c_byte]
_runtime._libdhd.dhdGetEncRange.restype = c_int
[docs]def getEncRange(
enc_min_out: MutableArray[int, int],
enc_max_out: MutableArray[int, int],
ID: int = -1
) -> int:
"""
Get the expected min and max encoder values for all axes
present on the current device. Axis indices that do not
exist on the device will return a range of 0.
:param MutableArray[int, int] encmin_out:
An output buffer to store the minimum encoder values for
each axis.
:param MutableArray[int, int] encmax_out:
An output buffer to store the maximum encoder values for
each axis.
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:raises TypeError:
If one of enc_min_out or enc_max_out either are not
subscriptable or do not support item assignment.
:raises IndexError:
If either enc_min_out or enc_max_out have length less
than :data:`forcedimension_core.constants.MAX_DOF`
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns:
0 on success, -1 otherwise.
"""
enc_min = (c_int * MAX_DOF)()
enc_max = (c_int * MAX_DOF)()
err = _runtime._libdhd.dhdGetEncRange(enc_min, enc_max, ID)
for i in range(MAX_DOF):
enc_min_out[i] = enc_min[i]
enc_max_out[i] = enc_max[i]
return err
_runtime._libdhd.dhdGetJointAngleRange.argtypes = [
c_double_ptr, c_double_ptr, c_byte
]
_runtime._libdhd.dhdGetJointAngleRange.restype = c_int
[docs]def getJointAngleRange(
jmin_out: MutableArray[int, float],
jmax_out: MutableArray[int, float],
ID: int = -1
) -> Tuple[
FloatDOFTuple, FloatDOFTuple, int
]:
"""
This function retrieves the expected min and max joint angles
(in [rad]) for all sensed degrees-of-freedom on the current
device. Axis indices that do not exist on the device will
return a range of 0.0.
:param MutableArray[int, int] jmin_out:
An output buffer to store the minimum encoder values for
each axis.
:param MutableArray[int, int] jmax_out:
An output buffer to store the maximum encoder values for
each axis.
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:raises TypeError:
If one of jmin_out or jmax_out either are not
subscriptable or do not support item assignment.
:raises IndexError:
If either jmin_out or jmax_out have length less than
:data:`forcedimension_core.constants.MAX_DOF`
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns:
0 on success and -1 otherwise.
"""
jmin = (c_double * MAX_DOF)()
jmax = (c_double * MAX_DOF)()
err = _runtime._libdhd.dhdGetJointAngleRange(
ct.cast(jmin, c_double_ptr), ct.cast(jmax, c_double_ptr), ID
)
for i in range(MAX_DOF):
jmin_out[i] = jmin[i]
jmax_out[i] = jmax[i]
return err
_runtime._libdhd.dhdControllerSetDevice.argtypes = [c_int, c_byte]
_runtime._libdhd.dhdControllerSetDevice.restype = c_int
[docs]def controllerSetDevice(devtype: DeviceType, ID: int = -1) -> int:
"""
If the connected device is a controller lets the programmer
define the Force Dimension mechanical structure attached to
it. Upon selecting a device model, the routine will attempt
to read that particular device configuration from the
controller. If this fails, a default configuration will be
selected and stored in the controller.
Note
----
This feature only applies to the following types of devices:
:data:`forcedimension_core.constants.DeviceType.CONTROLLER`
:data:`forcedimension_core.constants.DeviceType.CONTROLLER_HR`
:param DeviceType devtype:
The device type to use.
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns:
0 on success, -1 otherwise.
"""
return _runtime._libdhd.dhdControllerSetDevice(devtype, ID)
_runtime._libdhd.dhdReadConfigFromFile.argtypes = [c_char_p, c_byte]
_runtime._libdhd.dhdReadConfigFromFile.restype = c_int
[docs]def readConfigFromFile(filename: str, ID: int = -1):
"""
This function loads a specific device
calibration/configuration data from a file. Particularly
useful when using the generic controller connected to a Force
Dimension device without using the
:func:`forcedimension_core.dhd.controllerSetDevice()` call.
:param bytes filename:
Configuration file containing device
calibration/configuration data.
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns:
0 on success, -1 otherwise.
"""
return _runtime._libdhd.dhdReadConfigFromFile(filename.encode('utf-8'), ID)
_runtime._libdhd.dhdDeltaGravityJointTorques.argtypes = [
c_double, c_double, c_double,
c_double_ptr, c_double_ptr, c_double_ptr,
c_byte
]
_runtime._libdhd.dhdDeltaGravityJointTorques.restype = c_int
[docs]@typing_extensions.deprecated(
"This function is deprecated, please use "
"forcedimension_core.dhd.jointAnglesToGravityJointTorques() "
"instead."
)
def deltaGravityJointTorques(
joint_angles: Array[int, float],
out: MutableArray[int, float],
ID: int = -1
) -> int:
"""
Compute the DELTA joint torques required to compensate for
gravity in a given DELTA joint angle configuration. Please
refer to your device user manual for more information on
your device coordinate system.
.. deprecated:: 1.0.0
Force Dimension SDK v3.16.0+ recommends that you use
:func:`forcedimension_core.dhd.expert.jointAnglesToGravityJointTorques()`
instead.
:param Array[int, float] joint_angles:
Sequence of ``(j0, j1, j2)`` where ``j0``, ``j1``,
``j2`` refer to the joint angles for axis 0, 1, and 2,
respectively.
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:param MutableArray[int, float] out:
An output buffer to store the return.
:raises TypeError:
If ``out`` does not support item assignment either
because it's immutable or not subscriptable.
:raises IndexError:
If ``len(out) < 3``.
:raises ctypes.ArgumentError:
If any element of ``joint_angles`` is not implicitly
convertible to a C double.
:raises IndexError:
If ``len(joint_angles) < 3``.
:raises TypeError:
If ``joint_angles`` is not subscriptable.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns:
0 on success, -1 otherwise.
"""
q0 = c_double()
q1 = c_double()
q2 = c_double()
err = _runtime._libdhd.dhdDeltaGravityJointTorques(
joint_angles[0], joint_angles[1], joint_angles[2],
q0, q1, q2,
ID
)
out[0] = q0.value
out[1] = q1.value
out[2] = q2.value
return err
_runtime._libdhd.dhdWristGravityJointTorques.argtypes = [
c_double, c_double, c_double,
c_double_ptr, c_double_ptr, c_double_ptr,
c_byte
]
_runtime._libdhd.dhdWristGravityJointTorques.restype = c_int
[docs]@typing_extensions.deprecated(
"This function is deprecated, please use "
"forcedimension_core.dhd.jointAnglesToGravityJointTorques() "
"instead.",
)
def wristGravityJointTorques(
joint_angles: Array[int, float],
out: MutableArray[int, float],
ID: int = -1
) -> int:
"""
Compute the wrist joint torques required to compensate for
gravity in a given wrist joint angle configuration. Please
refer to your device user manual for more information on your
device coordinate system.
.. deprecated:: 1.0.0
Force Dimension SDK v3.16.0+ recommends that you use
:func:`forcedimension_core.dhd.jointAnglesToGravityJointTorques()`
instead.
:param Array[int, float] joint_angles:
Sequence of ``(j0, j1, j2)`` where ``j0``, ``j1``, and
``j2`` refer to the joint angles for wrist axes 0, 1,
and 2, respectively.
:param int ID:
Device ID (see :ref:`multiple_devices` section for details).
:param MutableArray[int, float] out:
An output buffer to store the joint torques.
:raises TypeError:
If ``out`` does not support item assignment either
because it's immutable or not subscriptable.
:raises IndexError:
If ``len(out) < 3``.
:raises ctypes.ArgumentError:
If any element of ``joint_angles`` is not implicitly
convertible to a C double.
:raises IndexError:
If ``len(joint_angles) < 3``.
:raises TypeError:
If ``joint_angles`` is not subscriptable.
:raises ctypes.ArgumentError:
If ``ID`` is not implicitly convertible to a C char.
:returns:
0 on success, -1 otherwise
"""
q0 = c_double()
q1 = c_double()
q2 = c_double()
err = _runtime._libdhd.dhdWristGravityJointTorques(
joint_angles[0], joint_angles[1], joint_angles[2],
q0, q1, q2,
ID
)
out[0] = q0.value
out[1] = q1.value
out[2] = q2.value
return err