Skip to content

Commit

Permalink
Merge pull request #39 from EmDash00/develop
Browse files Browse the repository at this point in the history
  • Loading branch information
@EmDash00
EmDash00 authored Jul 12, 2023
2 parents 2d31db7 + 8bc2b74 commit f9a5ff2
Show file tree
Hide file tree
Showing 4 changed files with 167 additions and 57 deletions.
110 changes: 66 additions & 44 deletions forcedimension/__init__.py
View file
Original file line number Diff line number Diff line change
@@ -1,3 +1,10 @@
"""
.. module::forcedimension
:platform: Windows, Unix
:synopsis: ForceDimensionSDK high level wrappers.
.. moduleauthor:: Ember "Emmy" Chow <emberchow.business@gmail.com>
"""
from math import nan
from threading import Condition, Lock, Thread
from time import monotonic, sleep
Expand All @@ -23,7 +30,7 @@
)


__version__ = '0.1.5'
__version__ = '0.1.6'


DefaultVecType = EuclidianVector
Expand Down Expand Up @@ -238,8 +245,9 @@ def pos(self) -> Optional[ImmutableWrapper[MutFSeq]]:
:rtype: Optional[MutableSequence[float]]
:returns: A mutable sequence of [x, y, z] where x, y, and z are the
end-effector's position given in [m].
:returns:
A mutable sequence of [x, y, z] where x, y, and z are the
end-effector's position given in [m].
"""
self.check_threadex()
return self._pos_view
Expand All @@ -266,8 +274,9 @@ def v(self) -> Optional[ImmutableWrapper[MutFSeq]]:
:rtype: Optional[MutableSequence[float]]
:returns: A mutable sequence of [vx, vy, vz] where vx, vy, and vz are
the end-effector's linear velocity given in [m/s].
:returns:
A mutable sequence of [vx, vy, vz] where vx, vy, and vz are
the end-effector's linear velocity given in [m/s].
"""
self.check_threadex()
return self._v_view
Expand All @@ -280,8 +289,9 @@ def w(self) -> Optional[ImmutableWrapper[MutFSeq]]:
:rtype: Optional[MutableSequence[float]]
:returns: A mutable sequence of [wx, wy, wz] where wx, wy, and wz are
the end-effector's linear velocity given in [rad/s].
:returns:
A mutable sequence of [wx, wy, wz] where wx, wy, and wz are
the end-effector's linear velocity given in [rad/s].
"""
self.check_threadex()
return self._w_view
Expand All @@ -294,8 +304,9 @@ def t(self) -> Optional[ImmutableWrapper[MutFSeq]]:
:rtype: Optional[MutableSequence[float]]
:returns: A mutable sequence of [tx, ty, tz] where tx, ty, and tz are
the torque experienced by the end-effector in [Nm]
:returns:
A mutable sequence of [tx, ty, tz] where tx, ty, and tz are
the torque experienced by the end-effector in [Nm]
"""
self.check_threadex()
return self._t_view
Expand All @@ -308,8 +319,9 @@ def f(self) -> Optional[ImmutableWrapper[MutFSeq]]:
:rtype: Optional[MutableSequence[float]]
:returns: A mutable sequence of [fx, fy, fz] where fx, fy, and fz are
the torque experienced by the end-effector in [N]
:returns:
A mutable sequence of [fx, fy, fz] where fx, fy, and fz are
the torque experienced by the end-effector in [N]
"""

self.check_threadex()
Expand Down Expand Up @@ -553,7 +565,7 @@ def submit(self):
See Also
--------
:func:HapticDevice.req
:func:`HapticDevice.req`
"""
self._req = False
Expand Down Expand Up @@ -692,11 +704,12 @@ def get_max_force(self) -> Optional[float]:
Retrieve the current limit (in N) to the force magnitude that can be
applied by the haptic device.
:returns: The current limit (in N) to the force magnitude that can be
applied by the haptic device to the end-effector. If there is no limit,
None is returned instead.
:returns:
The current limit (in N) to the force magnitude that can be
applied by the haptic device to the end-effector. If there is no
limit, None is returned instead.
rtype: Optional[float]
:rtype: Optional[float]
"""

limit = dhd.getMaxForce(ID=cast(int, self._id))
Expand All @@ -708,9 +721,9 @@ def set_max_force(self, limit: Optional[float]) -> None:
Define or disable a limit (in N) to the force magnitude that can be
applied by the haptic device.
:param Optional[float] limit: The desired limit (in N) to the force
magnitude that can be applied. If the limit is None, the force limit is
disabled.
:param Optional[float] limit:
The desired limit (in N) to the force magnitude that can be
applied. If the limit is None, the force limit is disabled.
"""
if limit is None:
err = dhd.setMaxForce(ID=cast(int, self._id), limit=-1.0)
Expand All @@ -728,11 +741,12 @@ def get_max_torque(self) -> Optional[float]:
Retrieve the current limit (in Nm) to the torque magnitude that can be
applied by the haptic device.
:returns: The current limit (in Nm) to the force magnitude that can be
applied by the haptic device to the end-effector. If there is no limit,
None is returned instead.
:returns:
The current limit (in Nm) to the force magnitude that can be
applied by the haptic device to the end-effector. If there is no
limit, None is returned instead.
rtype: Optional[float]
:rtype: Optional[float]
"""

limit = dhd.getMaxTorque(ID=cast(int, self._id))
Expand All @@ -744,9 +758,9 @@ def set_max_torque(self, limit: Optional[float]) -> None:
Define or disable a limit (in N) to the force magnitude that can be
applied by the haptic device.
:param Optional[float] limit: The desired limit (in N) to the force
magnitude that can be applied. If the limit is None, the force limit is
disabled.
:param Optional[float] limit:
The desired limit (in N) to the force magnitude that can be
applied. If the limit is None, the force limit is disabled.
"""
if limit is None:
err = dhd.setMaxTorque(ID=cast(int, self._id), limit=-1.0)
Expand Down Expand Up @@ -778,14 +792,15 @@ def get_button(self, button_id: int = 0) -> bool:
"""
See if the button on the device is being pressed.
See Also
--------
:class:`forcedimension.dhd.constants.NovintButtonID`
:param int button_id: The button to check
:rtype: bool
:returns: True if the button is being pressed, False otherwise.
See Also
--------
:class:forcedimension.dhd.dhd.constants.NovintButtonID
:returns: True if the button is being pressed, False otherwise
"""
return bool(self._buttons & cast(int, 1 << button_id))

Expand All @@ -811,6 +826,12 @@ class Gripper:
case, a Gripper object will be instantiated as well containing methods to
get kinematic information about the Gripper.
"""
_enc: int
_angle: float
_gap: float
_v: float
_w: float
_fg: float

def __init__(
self,
Expand All @@ -827,12 +848,12 @@ def __init__(

VecType = vecgen

self._enc: int = 0
self._angle: float = nan
self._gap: float = nan
self._v: float = nan
self._w: float = nan
self._fg: float = nan
self._enc = 0
self._angle = nan
self._gap = nan
self._v = nan
self._w = nan
self._fg = nan

self._thumb_pos: MutFSeq = VecType()
self._finger_pos: MutFSeq = VecType()
Expand Down Expand Up @@ -883,11 +904,12 @@ def get_max_force(self) -> Optional[float]:
Retrieve the current limit (in N) to the force magnitude that can be
applied by the haptic device.
:returns: The current limit (in N) to the force magnitude that can be
applied by the haptic device to the end-effector. If there is no limit,
None is returned instead.
:returns:
The current limit (in N) to the force magnitude that can be
applied by the haptic device to the end-effector. If there is no
limit, None is returned instead.
rtype: Optional[float]
:rtype: Optional[float]
"""

limit = dhd.getMaxGripperForce(ID=cast(int, self._id))
Expand All @@ -899,9 +921,9 @@ def set_max_force(self, limit: Optional[float]) -> None:
Define or disable a limit (in N) to the force magnitude that can be
applied by the haptic device.
:param Optional[float] limit: The desired limit (in N) to the force
magnitude that can be applied. If the limit is None, the force limit is
disabled.
:param Optional[float] limit:
The desired limit (in N) to the force magnitude that can be
applied. If the limit is None, the force limit is disabled.
"""
if limit is None:
err = dhd.setMaxGripperForce(ID=cast(int, self._id), limit=-1.0)
Expand Down
97 changes: 97 additions & 0 deletions forcedimension/dhd/__init__.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2536,6 +2536,103 @@ def setForceAndTorqueAndGripperForce(f: FloatVectorLike,
)


_libdhd.dhdGetForceAndTorqueAndGripperForce.argtypes = [
POINTER(c_double),
POINTER(c_double),
POINTER(c_double),
POINTER(c_double),
POINTER(c_double),
POINTER(c_double),
POINTER(c_double),
c_byte
]
_libdhd.dhdGetForceAndTorqueAndGripperForce.restype = c_int

def getForceAndTorqueAndGripperForce(
ID: int = -1,
f_out: Optional[MutableFloatVectorLike] = None,
t_out: Optional[MutableFloatVectorLike] = None
) -> Tuple[
Union[MutableFloatVectorLike, List[float]],
Union[MutableFloatVectorLike, List[float]],
float,
int
]:
"""
Retrieve the force and torque vectors applied to the device end-effector.
:param int ID:
Device ID (see multiple devices section for details), defaults to -1.
:param Optional[MutableFloatVectorLike] f_out:
An output buffer to store the applied forces on the end-effector. If
specified, the return will contain a reference to this buffer rather to
a newly allocated list, optional.
:param Optional[MutableFloatVectorLike] t_out:
An output buffer to store the applied torques on the end-effector. If
specified, the return will contain a reference to this buffer rather to
a newly allocated list, optional.
:raises ValueError:
If ``ID`` is not implicitly convertible to C char.
:returns:
A tuple in the form ``([fx, fy, fz], [tx, ty, tz], fg, err)``.
``[fx, fy, fz]`` are translation forces applied to the end-effector
(in [N]) about the X, Y, and Z axes, respectively. ``[tx, ty, tz]``
refers to the torques applied to the end-effector (in [Nm]) about
the X, Y, and Z axes. ``fg`` refers to the gripper force (in [Nm]).
``err`` is 0, on success, -1 otherwise.
:rtype:
Tuple
[
Union[MutableFloatVectorLike, List[float]],
Union[MutableFloatVectorLike, List[float]],
int
]
"""

fx = c_double()
fy = c_double()
fz = c_double()

tx = c_double()
ty = c_double()
tz = c_double()

fg = c_double()

err = _libdhd.dhdGetForceAndTorqueAndGripperForce(
byref(fx), byref(fy), byref(fz),
byref(tx), byref(ty), byref(tz),
byref(fg),
ID
)

if f_out is None:
f_ret = [fx.value, fy.value, fz.value]
else:
f_ret = f_out

f_out[0] = fx.value
f_out[1] = fy.value
f_out[2] = fz.value

if t_out is None:
t_ret = [tx.value, ty.value, tz.value]
else:
t_ret = t_out

t_out[0] = tx.value
t_out[1] = ty.value
t_out[2] = tz.value

return (f_ret, t_ret, fg.value, err)


_libdhd.dhdConfigLinearVelocity.argtypes = [c_int, c_int, c_byte]
_libdhd.dhdConfigLinearVelocity.restype = c_int

Expand Down
13 changes: 2 additions & 11 deletions forcedimension/dhd/os_independent.py
View file
Original file line number Diff line number Diff line change
@@ -1,11 +1,3 @@
"""
.. module::expert
:platform: Windows, Unix
:synopsis: libdhd "OS Independent SDK" Python libdhd
.. moduleauthor:: Ember "Emmy" Chow <emberchow.business@gmail.com>
"""

from ctypes import c_bool, c_byte, c_double
from forcedimension import runtime

Expand Down Expand Up @@ -68,7 +60,7 @@ def getTime() -> float:


_libdhd.dhdSleep.argtypes = [c_double]
_libdhd.dhd.restype = None
_libdhd.dhdSleep.restype = None


def sleep(sec: float) -> None:
Expand All @@ -81,5 +73,4 @@ def sleep(sec: float) -> None:


def startThread():
# TODO implement startThread
pass
raise NotImplementedError()
4 changes: 2 additions & 2 deletions pyproject.toml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,8 +4,8 @@ build-backend = "setuptools.build_meta"

[project]
name = "forcedimension"
version = "0.1.5"
authors = [
version = "0.1.6"
authors = [
{ name="Ember Chow", email="emberchow.business@gmail.com" },
]
description = "Python Bindings for the ForceDimension SDK"
Expand Down

0 comments on commit f9a5ff2

Please sign in to comment.