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