def get_constraint(self):
soft_constraints = OrderedDict()
current_joint = self.get_input_joint_position(self.joint_name)
weight = self.get_input_float(self.weight)
parent_link = self.get_robot().get_parent_link_of_joint(self.joint_name)
parent_R_root = sw.rotation_of(self.get_fk(parent_link, self.get_robot().get_root()))
com__parent = sw.position_of(self.get_fk_evaluated(parent_link, self.object_name))
com__parent[3] = 0
com__parent = sw.scale(com__parent, 1)
g = sw.vector3(0, 0, -1)
g__parent = parent_R_root * g
axis_of_rotation = sw.vector3(*self.get_robot().get_joint_axis(self.joint_name))
l = sw.dot(g__parent, axis_of_rotation)
goal__parent = g__parent - sw.scale(axis_of_rotation, l)
goal__parent = sw.scale(goal__parent, 1)
goal_vel = sw.acos(sw.dot(com__parent, goal__parent))
ref_axis_of_rotation = sw.cross(com__parent, goal__parent)
goal_vel *= sw.sign(sw.dot(ref_axis_of_rotation, axis_of_rotation))
soft_constraints[str(self)] = SoftConstraint(lower=goal_vel, # sw.Min(goal_vel, 0),
upper=goal_vel, # sw.Max(goal_vel, 0),
weight=weight,
expression=current_joint)
return soft_constraints
def _create_frames_expressions(self):
for joint_name, urdf_joint in self._urdf_robot.joint_map.items():
if self.is_joint_controllable(joint_name):
joint_symbol = self.get_joint_position_symbol(joint_name)
if self.is_joint_mimic(joint_name):
multiplier = 1 if urdf_joint.mimic.multiplier is None else urdf_joint.mimic.multiplier
offset = 0 if urdf_joint.mimic.offset is None else urdf_joint.mimic.offset
joint_symbol = self.get_joint_position_symbol(urdf_joint.mimic.joint) * multiplier + offset
if self.is_joint_type_supported(joint_name):
if urdf_joint.origin is not None:
xyz = urdf_joint.origin.xyz if urdf_joint.origin.xyz is not None else [0, 0, 0]
rpy = urdf_joint.origin.rpy if urdf_joint.origin.rpy is not None else [0, 0, 0]
joint_frame = spw.translation3(*xyz) * spw.rotation_matrix_from_rpy(*rpy)
else:
joint_frame = spw.eye(4)
else:
# TODO more specific exception
raise TypeError(u'Joint type "{}" is not supported by urdfs parser.'.format(urdf_joint.type))
if self.is_rotational_joint(joint_name):
joint_frame *= spw.rotation_matrix_from_axis_angle(spw.vector3(*urdf_joint.axis), joint_symbol)
elif self.is_translational_joint(joint_name):
joint_frame *= spw.translation3(*(spw.point3(*urdf_joint.axis) * joint_symbol)[:3])
self._joint_to_frame[joint_name] = joint_frame
def get_expression(self):
return sw.vector3(self.x, self.y, self.z)