def set_open_goal(self,
tip_link,
object_name,
object_link_name,
root_link=None,
goal_joint_state=None,
weight=WEIGHT_ABOVE_CA):
"""
:type tip_link: str
:param tip_link: tip of manipulator (gripper) which is used
:type object_name str
:param object_name
:type object_link_name str
:param object_link_name handle to grasp
:type root_link: str
:param root_link: default is root link of robot
:type goal_joint_state: float
:param goal_joint_state: how far to open
"""
constraint = Constraint()
params = {
u'tip_link': tip_link,
u'object_name': object_name,
u'object_link_name': object_link_name
}
if root_link:
params[u'root_link'] = root_link
if goal_joint_state:
params[u'goal_joint_state'] = goal_joint_state
if weight:
params[u'weight'] = weight
constraint.parameter_value_pair = json.dumps(params)
self.cmd_seq[-1].constraints.append(constraint)
def set_json_goal(self, constraint_type, **kwargs):
constraint = Constraint()
constraint.type = constraint_type
for k, v in kwargs.items():
if isinstance(v, Message):
kwargs[k] = convert_ros_message_to_dictionary(v)
constraint.parameter_value_pair = json.dumps(kwargs)
self.cmd_seq[-1].constraints.append(constraint)
def set_json_goal(self, constraint_type, **kwargs):
"""
Set a goal for any of the goals defined in Constraints.py
:param constraint_type: Name of the Goal
:type constraint_type: str
:param kwargs: maps constraint parameter names to values. Values should be float, str or ros messages.
:type kwargs: dict
"""
constraint = Constraint()
constraint.type = constraint_type
for k, v in kwargs.items():
if isinstance(v, Message):
kwargs[k] = convert_ros_message_to_dictionary(v)
constraint.parameter_value_pair = json.dumps(kwargs)
self.cmd_seq[-1].constraints.append(constraint)
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_joint_goal(self, joint_state, weight=None, gain=None, max_speed=None):
"""
:param joint_state:
:type joint_state: dict
"""
if not weight and not gain and not max_speed:
constraint = JointConstraint()
constraint.type = JointConstraint.JOINT
if isinstance(joint_state, dict):
for joint_name, joint_position in joint_state.items():
constraint.goal_state.name.append(joint_name)
constraint.goal_state.position.append(joint_position)
elif isinstance(joint_state, JointState):
constraint.goal_state = joint_state
self.cmd_seq[-1].joint_constraints.append(constraint)
elif isinstance(joint_state, dict):
for joint_name, joint_position in joint_state.items():
constraint = Constraint()
constraint.type = u'JointPosition'
params = {}
params[u'joint_name'] = joint_name
params[u'goal'] = joint_position
if weight:
params[u'weight'] = weight
if gain:
params[u'gain'] = gain
if max_speed:
params[u'max_speed'] = max_speed
constraint.parameter_value_pair = json.dumps(params)
self.cmd_seq[-1].constraints.append(constraint)
def set_joint_goal(self, goal_state, weight=None, max_velocity=None):
"""
This goal will move the robots joint to the desired configuration.
:param goal_state: Can either be a joint state messages or a dict mapping joint name to position.
:type goal_state: Union[JointState, dict]
:param weight: default WEIGHT_BELOW_CA
:type weight: float
:param max_velocity: default is the default of the added joint goals
:type max_velocity: float
"""
if weight is None and max_velocity is None:
constraint = JointConstraint()
constraint.type = JointConstraint.JOINT
if isinstance(goal_state, JointState):
constraint.goal_state = goal_state
else:
for joint_name, joint_position in goal_state.items():
constraint.goal_state.name.append(joint_name)
constraint.goal_state.position.append(joint_position)
self.cmd_seq[-1].joint_constraints.append(constraint)
else:
constraint = Constraint()
constraint.type = JointConstraint.JOINT
if isinstance(goal_state, JointState):
goal_state = goal_state
else:
goal_state2 = JointState()
for joint_name, joint_position in goal_state.items():
goal_state2.name.append(joint_name)
goal_state2.position.append(joint_position)
goal_state = goal_state2
params = {}
params[u'goal_state'] = convert_ros_message_to_dictionary(
goal_state)
if weight is not None:
params[u'weight'] = weight
if max_velocity is not None:
params[u'max_velocity'] = max_velocity
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)
def set_json_goal(self, constraint_type, **kwargs):
constraint = Constraint()
constraint.type = constraint_type
constraint.parameter_value_pair = json.dumps(kwargs)
self.cmd_seq[-1].constraints.append(constraint)