def set_rotation_goal(self, root, tip, pose_stamped, weight=None, gain=None, max_speed=None):
"""
:param tip:
:type tip: str
:param pose_stamped:
:type pose_stamped: PoseStamped
"""
if not gain and not max_speed and not weight:
constraint = CartesianConstraint()
constraint.type = CartesianConstraint.ROTATION_3D
constraint.root_link = str(root)
constraint.tip_link = str(tip)
constraint.goal = pose_stamped
self.cmd_seq[-1].cartesian_constraints.append(constraint)
else:
constraint = Constraint()
constraint.type = u'CartesianOrientationSlerp'
params = {}
params[u'root_link'] = root
params[u'tip_link'] = tip
params[u'goal'] = convert_ros_message_to_dictionary(pose_stamped)
if gain:
params[u'gain'] = gain
if max_speed:
params[u'max_speed'] = max_speed
if weight:
params[u'weight'] = weight
constraint.parameter_value_pair = json.dumps(params)
self.cmd_seq[-1].constraints.append(constraint)
def set_straight_translation_goal(self,
goal_pose,
tip_link,
root_link,
weight=None,
max_velocity=None):
"""
This goal will use the kinematic chain between root and tip link to move tip link on the straightest
line into the goal position
:param root_link: name of the root link of the kin chain
:type root_link: str
:param tip_link: name of the tip link of the kin chain
:type tip_link: str
:param goal_pose: the goal pose, orientation will be ignored
:type goal_pose: PoseStamped
:param max_velocity: m/s, default 0.1
:type max_velocity: float
:param weight: default WEIGHT_ABOVE_CA
:type weight: float
"""
if not max_velocity and not weight:
constraint = CartesianConstraint()
constraint.type = CartesianConstraint.STRAIGHT_TRANSLATION_3D
constraint.root_link = str(root_link)
constraint.tip_link = str(tip_link)
constraint.goal = goal_pose
self.cmd_seq[-1].cartesian_constraints.append(constraint)
else:
constraint = Constraint()
constraint.type = u'CartesianPositionStraight'
params = {}
params[u'root_link'] = root_link
params[u'tip_link'] = tip_link
params[u'goal'] = convert_ros_message_to_dictionary(goal_pose)
if max_velocity:
params[u'max_velocity'] = max_velocity
if weight:
params[u'weight'] = weight
constraint.parameter_value_pair = json.dumps(params)
self.cmd_seq[-1].constraints.append(constraint)