def send_move_group_joint_space_goal(self, move_group, joint_names, joint_values, plan_only=None):
'''
Retrieve the current move group action configuration and send the move request
@param move_group : string specifying a particular move group
@param joint_names : list of joint names for command
@param joint_values : list of joint values for target
'''
# We should have already set this pairing up during initialization
action_topic = ProxyMoveItClient._move_group_clients_[move_group]
# Get the latest setup of of the MoveGroup goal data
goal = MoveGroupGoal()
goal.request = copy.deepcopy(ProxyMoveItClient._motion_plan_requests[move_group])
goal.planning_options = copy.deepcopy(ProxyMoveItClient._planning_options[move_group])
if (plan_only is not None):
goal.planning_options.plan_only = plan_only
goal_constraints = Constraints()
for i in range(len(joint_names)):
jc = ProxyMoveItClient._joint_constraints[move_group][joint_names[i]]
goal_constraints.joint_constraints.append(JointConstraint(
joint_name=joint_names[i],
position=joint_values[i],
tolerance_above=jc.tolerance_above,
tolerance_below=jc.tolerance_above,
weight=jc.weight))
goal.request.goal_constraints.append(goal_constraints)
ProxyMoveItClient._action_clients[action_topic].send_goal(action_topic, goal)
def make_moveit_action_goal(joint_names, joint_positions):
goal_config_constraint = Constraints()
for name, position in zip(joint_names, joint_positions):
joint_constraint = JointConstraint()
joint_constraint.joint_name = name
joint_constraint.position = position
goal_config_constraint.joint_constraints.append(joint_constraint)
req = MotionPlanRequest()
req.group_name = 'both_arms'
req.goal_constraints.append(goal_config_constraint)
goal = MoveGroupGoal()
goal.request = req
return goal
jc0.tolerance_below = 0.0001
jc0.weight = 1.0
cons = Constraints()
cons.name = ""
cons.joint_constraints.append(jc2)
cons.joint_constraints.append(jc3)
cons.joint_constraints.append(jc4)
cons.joint_constraints.append(jc5)
cons.joint_constraints.append(jc1)
cons.joint_constraints.append(jc0)
msg.workspace_parameters = workspace_parameters
msg.start_state = start_state
msg.goal_constraints.append(cons)
msg.num_planning_attempts = 1
msg.allowed_planning_time = 5.0
msg.group_name = "manipulator"
move_group_goal.request = msg
client.send_goal(move_group_goal)
client.wait_for_result(rospy.Duration.from_sec(5.0))
except rospy.ROSInterruptException, e:
print "failed: %s"%e
def joint_position_callback(joints):
global plan_only
fixed_frame = rospy.get_param("/fixed_frame")
client = actionlib.SimpleActionClient('move_group', MoveGroupAction)
client.wait_for_server()
move_group_goal = MoveGroupGoal()
try:
msg = MotionPlanRequest()
workspace_parameters = WorkspaceParameters()
workspace_parameters.header.stamp = rospy.Time.now()
workspace_parameters.header.frame_id = fixed_frame
workspace_parameters.min_corner = Vector3(-1.0, -1.0, -1.0)
workspace_parameters.max_corner = Vector3(1.0, 1.0, 1.0)
start_state = RobotState()
# start_state.joint_state.header.stamp = rospy.Time.now()
start_state.joint_state.header.frame_id = fixed_frame
start_state.joint_state.name = []
start_state.joint_state.position = []
cons = Constraints()
cons.name = ""
i = 0
for dim in joints.start_joint.layout.dim:
start_state.joint_state.name.append(dim.label)
start_state.joint_state.position.append(joints.start_joint.data[i])
jc = JointConstraint()
jc.joint_name = dim.label
jc.position = joints.goal_joint.data[i]
jc.tolerance_above = 0.0001
jc.tolerance_below = 0.0001
jc.weight = 1.0
i = i + 1
cons.joint_constraints.append(jc)
msg.workspace_parameters = workspace_parameters
msg.start_state = start_state
msg.goal_constraints.append(cons)
msg.num_planning_attempts = 1
msg.allowed_planning_time = 5.0
msg.group_name = joints.group_name
move_group_goal.request = msg
if joints.plan_only:
plan_only = True
move_group_goal.planning_options.plan_only = True
else:
plan_only = False
client.send_goal(move_group_goal)
client.wait_for_result(rospy.Duration.from_sec(5.0))
except rospy.ROSInterruptException, e:
print "failed: %s"%e
jc0.position = -4.43859335239
jc0.tolerance_above = 0.0001
jc0.tolerance_below = 0.0001
jc0.weight = 1.0
cons = Constraints()
cons.name = ""
cons.joint_constraints.append(jc2)
cons.joint_constraints.append(jc3)
cons.joint_constraints.append(jc4)
cons.joint_constraints.append(jc5)
cons.joint_constraints.append(jc1)
cons.joint_constraints.append(jc0)
msg.workspace_parameters = workspace_parameters
msg.start_state = start_state
msg.goal_constraints.append(cons)
msg.num_planning_attempts = 1
msg.allowed_planning_time = 5.0
msg.group_name = "manipulator"
move_group_goal.request = msg
client.send_goal(move_group_goal)
client.wait_for_result(rospy.Duration.from_sec(5.0))
except rospy.ROSInterruptException, e:
print "failed: %s" % e
def joint_position_callback(joints):
global plan_only
fixed_frame = rospy.get_param("/fixed_frame")
client = actionlib.SimpleActionClient('move_group', MoveGroupAction)
client.wait_for_server()
move_group_goal = MoveGroupGoal()
try:
msg = MotionPlanRequest()
workspace_parameters = WorkspaceParameters()
workspace_parameters.header.stamp = rospy.Time.now()
workspace_parameters.header.frame_id = fixed_frame
workspace_parameters.min_corner = Vector3(-1.0, -1.0, -1.0)
workspace_parameters.max_corner = Vector3(1.0, 1.0, 1.0)
start_state = RobotState()
# start_state.joint_state.header.stamp = rospy.Time.now()
start_state.joint_state.header.frame_id = fixed_frame
start_state.joint_state.name = []
start_state.joint_state.position = []
cons = Constraints()
cons.name = ""
i = 0
for dim in joints.start_joint.layout.dim:
start_state.joint_state.name.append(dim.label)
start_state.joint_state.position.append(joints.start_joint.data[i])
jc = JointConstraint()
jc.joint_name = dim.label
jc.position = joints.goal_joint.data[i]
jc.tolerance_above = 0.0001
jc.tolerance_below = 0.0001
jc.weight = 1.0
i = i + 1
cons.joint_constraints.append(jc)
msg.workspace_parameters = workspace_parameters
msg.start_state = start_state
msg.goal_constraints.append(cons)
msg.num_planning_attempts = 1
msg.allowed_planning_time = 5.0
msg.group_name = joints.group_name
move_group_goal.request = msg
if joints.plan_only:
plan_only = True
move_group_goal.planning_options.plan_only = True
else:
plan_only = False
client.send_goal(move_group_goal)
client.wait_for_result(rospy.Duration.from_sec(5.0))
except rospy.ROSInterruptException, e:
print "failed: %s" % e