def __init__(self, centerOfMass, inputToJoint, jointRotationAxis,
jointToOutput):
centerOfMassP = (ctypes.c_float * 3)(*centerOfMass)
inputToJointP = _MATRIX(*flatten(inputToJoint))
jointRotationAxisP = (ctypes.c_float * 3)(*jointRotationAxis)
jointToOutputP = _MATRIX(*flatten(jointToOutput))
addr = hebi.hebiActuatorCreate(centerOfMassP, inputToJointP,
jointRotationAxisP, jointToOutputP)
super().__init__(addr)
def setTorques(self, torques, send=True):
'''Sets the output torques of the modules in the group.
The length of torques must be the length of the modules in the group.
If a torque is None, the corresponding module is not sent an torque
command.
Args:
torques (float list): The target torques
send (bool): True sends the command immediately, and
False only updates the internal
HebiGroupCommand object.
'''
if len(torques) != self.getNumModules():
raise HebiAccessError(
'Length of torques given was {} but group contains {} modules.'
.format(len(torques), self.getNumModules())
)
torques = flatten(torques)
commands = self.groupCommand.getCommandList()
for i, torque in enumerate(torques):
if torque is not None:
commands[i].setField(Commands.CommandFloatTorque, torque)
if send:
self.sendCommand()
def getJacobians(self, frameType, positions):
'''Generates the jacobian for each frame for the kinematic tree.
Args:
frameType (kinematics enum): Which type fo frame to consider.
positions (float list): A list of joint positions equal in length to the number of DoFs of the kinematic tree.
'''
dofs = self.getNumberOfDoFs()
frameSize = 6 * dofs
positionsPointer = ctypes.pointer(
(len(positions) * ctypes.c_double)(*flatten(positions)))
jacobiansPointer = ctypes.pointer(
((self.getNumberOfFrames(frameType) * frameSize) *
ctypes.c_float)())
hebi.hebiKinematicsGetJacobians(self.getAddress(), frameType.value,
positionsPointer, jacobiansPointer)
jacobians = list(jacobiansPointer.contents)
jacobians = [
jacobians[i:i + frameSize]
for i in range(0, len(jacobians), frameSize)
]
jacobians = [[frame[i:i + dofs] for i in range(0, len(frame), dofs)]
for frame in jacobians][-1]
return returnMatrix(jacobians)
def setBaseFrame(self, transform):
'''Sets the fixed transform from the origin to the first added body.
Args:
transform (float): A 4x4 homogeneous transform.
'''
transformPointer = ctypes.pointer(_MATRIX(*flatten(transform)))
hebi.hebiKinematicsSetBaseFrame(self.getAddress(), transformPointer)
def __setitem__(self, *args, **kwargs):
key, value = args[:2] # pylint: disable=unbalanced-tuple-unpacking
if key in self.keys():
args = list(args)
args[1] = flatten(value)
previous = _Gains(self.callback, self)
dict.__setitem__(self, *args, **kwargs)
self.callback(previous, args[1])
else:
raise HebiAccessError('Cannot add additional gains!')
def setEnums(self, field, values):
'''Sets the field of all the modules to the ints in the values array.
Should not normally be used directly, proceed with caution.
Args:
field (int): the field to be set
value (int array): the values to set'''
values = flatten(values)
for command, value in zip(self.getCommandList(), values):
command.setEnum(field, value)
def getForwardKinematics(self, frameType, positions):
'''Generates the forward kinematics for the kinematic tree.
The order of the returned frames is in a depth-first tree.
Args:
frameType (kinematics enum): Which type of frame to consider.
positions (float list): A list of joint positions equal in length to the number of DoFs of the kinematic tree.
'''
framesP = ctypes.pointer(
(ctypes.c_float * (16 * self.getNumberOfFrames(frameType)))())
positions = (ctypes.c_double *
self.getNumberOfDoFs())(*flatten(positions))
hebi.hebiKinematicsGetForwardKinematics(self.getAddress(),
frameType.value, positions,
framesP)
frames = list(framesP.contents)
return returnMatrix(
[[frames[i:i + 16][j:4 + j] for j in range(0, 16, 4)]
for i in range(0, len(frames), 16)])
def __init__(self, centerOfMass, outputs):
centerOfMassP = ctypes.pointer((ctypes.c_float * 3)(*centerOfMass))
outputsP = ctypes.pointer(
(ctypes.c_float * (16 * len(outputs)))(*flatten(outputs)))
addr = hebi.hebiStaticBodyCreate(centerOfMassP, len(outputs), outputsP)
super().__init__(addr)