def set_goal(self, point, invalid_goal_callback, fail_out_of_range_goal,
**kwargs):
"""Sets goal for the joint group
Sets and validates the goal point for the joints
in this command group.
Parameters
----------
point: trajectory_msgs.JointTrajectoryPoint
the target point for all joints
invalid_goal_callback: func
error callback for invalid goal
fail_out_of_range_goal: bool
whether to bound out-of-range goals to range or fail
Returns
-------
bool
False if commanded goal invalid, else True
"""
self.goal = {
"position": None,
"velocity": None,
"acceleration": None,
"contact_threshold": None
}
if self.active:
goal_pos = point.positions[self.index] if len(
point.positions) > self.index else None
if goal_pos is None:
err_str = (
"Received goal point with positions array length={0}. "
"This joint ({1})'s index is {2}. Length of array must cover all joints listed "
"in commanded_joint_names.").format(
len(point.positions), self.name, self.index)
invalid_goal_callback(err_str)
return False
self.goal['position'] = hm.bound_ros_command(
self.range, goal_pos, fail_out_of_range_goal)
self.goal['velocity'] = point.velocities[self.index] if len(
point.velocities) > self.index else None
self.goal['acceleration'] = point.accelerations[self.index] if len(
point.accelerations) > self.index else None
self.goal['contact_threshold'] = point.effort[self.index] if len(
point.effort) > self.index else None
if self.goal['position'] is None:
err_str = ("Received {0} goal point that is out of bounds. "
"Range = {1}, but goal point = {2}.").format(
self.name, self.range, goal_pos)
invalid_goal_callback(err_str)
return False
return True
def set_goal(self, point, invalid_goal_callback, fail_out_of_range_goal,
**kwargs):
self.goal = {
"position": None,
"velocity": None,
"acceleration": None,
"contact_threshold": None
}
if self.active:
if self.is_telescoping:
goal_pos = sum([point.positions[i] for i in self.index]) \
if len(point.positions) > max(self.index) else None
self.goal['velocity'] = point.velocities[self.index[0]] \
if len(point.velocities) > self.index[0] else None
self.goal['acceleration'] = point.accelerations[self.index[0]] \
if len(point.accelerations) > self.index[0] else None
self.goal['contact_threshold'] = point.effort[self.index[0]] \
if len(point.effort) > self.index[0] else None
else:
goal_pos = point.positions[self.index] \
if len(point.positions) > self.index else None
self.goal['velocity'] = point.velocities[self.index] \
if len(point.velocities) > self.index else None
self.goal['acceleration'] = point.accelerations[self.index] \
if len(point.accelerations) > self.index else None
self.goal['contact_threshold'] = point.effort[self.index] \
if len(point.effort) > self.index else None
if goal_pos is None:
err_str = (
"Received goal point with positions array length={0}. "
"This joint ({1})'s index is {2}. Length of array must cover all joints listed "
"in commanded_joint_names.").format(
len(point.positions), self.name, self.index)
invalid_goal_callback(err_str)
return False
self.goal['position'] = hm.bound_ros_command(
self.range, goal_pos, fail_out_of_range_goal)
if self.goal['position'] is None:
err_str = ("Received {0} goal point that is out of bounds. "
"Range = {1}, but goal point = {2}.").format(
self.name, self.range, goal_pos)
invalid_goal_callback(err_str)
return False
return True
def set_goal(self, point, invalid_goal_callback, **kwargs):
self.goal = {"position": None, "velocity": None, "acceleration": None, "contact_threshold": None}
if self.active:
goal_pos = point.positions[self.index] if len(point.positions) > self.index else None
if goal_pos is None:
err_str = ("Received goal point with positions array length={0}. "
"This joint ({1})'s index is {2}. Length of array must cover all joints listed "
"in commanded_joint_names.").format(len(point.positions), self.name, self.index)
invalid_goal_callback(err_str)
return False
joint_range = self.range_aperture_m if (self.name == 'gripper_aperture') else self.range_finger_rad
self.goal['position'] = hm.bound_ros_command(joint_range, goal_pos)
self.goal['velocity'] = point.velocities[self.index] if len(point.velocities) > self.index else None
self.goal['acceleration'] = point.accelerations[self.index] if len(point.accelerations) > self.index else None
self.goal['contact_threshold'] = point.effort[self.index] if len(point.effort) > self.index else None
if self.goal['position'] is None:
err_str = ("Received {0} goal point that is out of bounds. "
"Range = {1}, but goal point = {2}.").format(self.name, joint_range, goal_pos)
invalid_goal_callback(err_str)
return False
return True
def ros_to_mechaduino(self, ros_ros, manipulation_origin):
ros_pos = hm.bound_ros_command(self.range, ros_ros)
return (manipulation_origin['x'] + ros_pos) if ros_pos is not None else None
