def limit_base_joint_constraint(): ur5_constraints = Constraints() shoulder_pan_joint_constraint = JointConstraint() shoulder_lift_joint_constraint = JointConstraint() wrist_1_joint_constraint = JointConstraint() shoulder_pan_joint_constraint.joint_name = "shoulder_pan_joint" shoulder_pan_joint_constraint.position = 0 shoulder_pan_joint_constraint.tolerance_above = 3.14 shoulder_pan_joint_constraint.tolerance_below = 3.14 shoulder_pan_joint_constraint.weight = 1 shoulder_lift_joint_constraint.joint_name = "shoulder_lift_joint" shoulder_lift_joint_constraint.position = 0 shoulder_lift_joint_constraint.tolerance_above = 0 shoulder_lift_joint_constraint.tolerance_below = 3.14 shoulder_lift_joint_constraint.weight = 1 wrist_1_joint_constraint.joint_name = "wrist_1_joint" wrist_1_joint_constraint.position = 0 wrist_1_joint_constraint.tolerance_above = 3.14 wrist_1_joint_constraint.tolerance_below = 3.14 wrist_1_joint_constraint.weight = 1 ur5_constraints.joint_constraints.append(shoulder_pan_joint_constraint) ur5_constraints.joint_constraints.append(shoulder_lift_joint_constraint) ur5_constraints.joint_constraints.append(wrist_1_joint_constraint) return ur5_constraints
def set_pose_constraints(self, tol_joint1, tol_joint2, tol_joint3): angle_constraints = Constraints() joint_constraint = JointConstraint() angle_constraints.name = "angle" joint_constraint.position = self.joint1_con joint_constraint.tolerance_above = tol_joint1 joint_constraint.tolerance_below = tol_joint1 joint_constraint.weight = 0.1 joint_constraint.joint_name = "arm_shoulder_pan_joint" angle_constraints.joint_constraints.append(joint_constraint) joint_constraint2 = JointConstraint() joint_constraint2.position = self.joint2_con joint_constraint2.tolerance_above = tol_joint2 joint_constraint2.tolerance_below = tol_joint2 joint_constraint2.weight = 0.2 joint_constraint2.joint_name = "arm_wrist_2_joint" angle_constraints.joint_constraints.append(joint_constraint2) joint_constraint3 = JointConstraint() joint_constraint3.position = self.joint3_con joint_constraint3.tolerance_above = tol_joint3 joint_constraint3.tolerance_below = tol_joint3 joint_constraint3.weight = 0.3 joint_constraint3.joint_name = "arm_wrist_1_joint" angle_constraints.joint_constraints.append(joint_constraint3) group.set_path_constraints(angle_constraints)
def main(argv): client = actionlib.SimpleActionClient( '/plan_and_execute_pose_w_joint_constraints', PlanExecutePoseConstraintsAction) client.wait_for_server() goal = PlanExecutePoseConstraintsGoal() try: goal.target_pose.header.frame_id = argv[0] goal.target_pose.pose.position.x = float(argv[1]) goal.target_pose.pose.position.y = float(argv[2]) goal.target_pose.pose.position.z = float(argv[3]) roll = float(argv[4]) * math.pi / 180.0 pitch = float(argv[5]) * math.pi / 180.0 yaw = float(argv[6]) * math.pi / 180.0 quat = quaternion_from_euler(roll, pitch, yaw) print quat goal.target_pose.pose.orientation.x = quat[0] goal.target_pose.pose.orientation.y = quat[1] goal.target_pose.pose.orientation.z = quat[2] goal.target_pose.pose.orientation.w = quat[3] joint_constraint_arm1_joint = JointConstraint() joint_constraint_arm1_joint.joint_name = "arm1_joint" joint_constraint_arm1_joint.position = float(0.0) joint_constraint_arm1_joint.tolerance_above = float( 30) * math.pi / 180.0 joint_constraint_arm1_joint.tolerance_below = float( 30) * math.pi / 180.0 joint_constraint_arm1_joint.weight = 1.0 joint_constraint_arm6_joint = JointConstraint() joint_constraint_arm6_joint.joint_name = "arm6_joint" joint_constraint_arm6_joint.position = float(0.0) joint_constraint_arm6_joint.tolerance_above = float( 10) * math.pi / 180.0 joint_constraint_arm6_joint.tolerance_below = float( 10) * math.pi / 180.0 joint_constraint_arm6_joint.weight = 1.0 goal.joint_constraints.append(joint_constraint_arm1_joint) goal.joint_constraints.append(joint_constraint_arm6_joint) except ValueError: quit() client.send_goal(goal) client.wait_for_result() print client.get_result()
def add_robot_constraints(): constraint = Constraints() constraint.name = "camera constraint" roll_constraint = OrientationConstraint() # 'base_link' is equal to the world link roll_constraint.header.frame_id = 'world' # The link that must be oriented upwards roll_constraint.link_name = "camera" roll_constraint.orientation = Quaternion(*tf.transformations.quaternion_from_euler(0,np.pi/3,0)) # Allow rotation of 45 degrees around the x and y axis roll_constraint.absolute_x_axis_tolerance = np.pi #Allow max rotation of x degrees roll_constraint.absolute_y_axis_tolerance = np.pi roll_constraint.absolute_z_axis_tolerance = np.pi/2 # The roll constraint is the only constraint roll_constraint.weight = 1 #constraint.orientation_constraints = [roll_constraint] joint_1_constraint = JointConstraint() joint_1_constraint.joint_name = "joint_1" joint_1_constraint.position = 0 joint_1_constraint.tolerance_above = np.pi/2 joint_1_constraint.tolerance_below = np.pi/2 joint_1_constraint.weight = 1 joint_4_constraint = JointConstraint() joint_4_constraint.joint_name = "joint_4" joint_4_constraint.position = 0 joint_4_constraint.tolerance_above = np.pi/2 joint_4_constraint.tolerance_below = np.pi/2 joint_4_constraint.weight = 1 joint_5_constraint = JointConstraint() joint_5_constraint.joint_name = "joint_5" joint_5_constraint.position = np.pi/2 joint_5_constraint.tolerance_above = np.pi/2 joint_5_constraint.tolerance_below = np.pi/2 joint_5_constraint.weight = 1 joint_6_constraint = JointConstraint() joint_6_constraint.joint_name = "joint_6" joint_6_constraint.position = np.pi-0.79 joint_6_constraint.tolerance_above = np.pi joint_6_constraint.tolerance_below = np.pi joint_6_constraint.weight = 1 constraint.joint_constraints = [joint_1_constraint, joint_6_constraint] return constraint
def execute(self, userdata): header = Header() header.frame_id = 'base_link' header.stamp = rospy.Time.now() userdata.move_it_joint_goal = MoveGroupGoal() goal_c = Constraints() for name, value in zip(userdata.manip_joint_names, userdata.manip_goal_joint_pose): joint_c = JointConstraint() joint_c.joint_name = name joint_c.position = value joint_c.tolerance_above = 0.01 joint_c.tolerance_below = 0.01 joint_c.weight = 1.0 goal_c.joint_constraints.append(joint_c) userdata.move_it_joint_goal.request.goal_constraints.append(goal_c) userdata.move_it_joint_goal.request.num_planning_attempts = 5 userdata.move_it_joint_goal.request.allowed_planning_time = 5.0 userdata.move_it_joint_goal.planning_options.plan_only = False #False = Plan + Execute ; True = Plan only userdata.move_it_joint_goal.planning_options.planning_scene_diff.is_diff = True userdata.move_it_joint_goal.request.group_name = userdata.manip_joint_group rospy.loginfo('Group Name: ' + userdata.manip_joint_group) rospy.loginfo('Joints name: ' + str(userdata.manip_joint_names)) rospy.loginfo('Joints Values: ' + str(userdata.manip_goal_joint_pose)) rospy.loginfo('GOAL TO SEND IS:... ' + str(userdata.move_it_joint_goal)) return 'succeeded'
def create_move_group_joints_goal(self, joint_names, joint_values, group="right_arm", plan_only=False): """ Creates a move_group goal based on pose. @arg joint_names list of strings of the joint names @arg joint_values list of digits with the joint values @arg group string representing the move_group group to use @arg plan_only bool to for only planning or planning and executing @return MoveGroupGoal with the desired contents""" header = Header() header.frame_id = 'base_link' header.stamp = rospy.Time.now() moveit_goal = MoveGroupGoal() goal_c = Constraints() for name, value in zip(joint_names, joint_values): joint_c = JointConstraint() joint_c.joint_name = name joint_c.position = value joint_c.tolerance_above = 0.01 joint_c.tolerance_below = 0.01 joint_c.weight = 1.0 goal_c.joint_constraints.append(joint_c) moveit_goal.request.goal_constraints.append(goal_c) moveit_goal.request.num_planning_attempts = 5 moveit_goal.request.allowed_planning_time = 5.0 moveit_goal.planning_options.plan_only = plan_only moveit_goal.planning_options.planning_scene_diff.is_diff = True moveit_goal.request.group_name = group return moveit_goal
def create_move_group_joints_goal(joint_names, joint_values, group="right_arm_torso", plan_only=True): """ Creates a move_group goal based on pose. @arg joint_names list of strings of the joint names @arg joint_values list of digits with the joint values @arg group string representing the move_group group to use @arg plan_only bool to for only planning or planning and executing @return MoveGroupGoal with the desired contents""" header = Header() header.frame_id = 'base_link' header.stamp = rospy.Time.now() moveit_goal = MoveGroupGoal() goal_c = Constraints() for name, value in zip(joint_names, joint_values): joint_c = JointConstraint() joint_c.joint_name = name joint_c.position = value joint_c.tolerance_above = 0.01 joint_c.tolerance_below = 0.01 joint_c.weight = 1.0 goal_c.joint_constraints.append(joint_c) moveit_goal.request.goal_constraints.append(goal_c) moveit_goal.request.num_planning_attempts = 1 moveit_goal.request.allowed_planning_time = 5.0 moveit_goal.planning_options.plan_only = plan_only moveit_goal.planning_options.planning_scene_diff.is_diff = True moveit_goal.request.group_name = group return moveit_goal
def execute(self, userdata): header = Header() header.frame_id = "base_link" header.stamp = rospy.Time.now() userdata.move_it_joint_goal = MoveGroupGoal() goal_c = Constraints() for name, value in zip(userdata.manip_joint_names, userdata.manip_goal_joint_pose): joint_c = JointConstraint() joint_c.joint_name = name joint_c.position = value joint_c.tolerance_above = 0.01 joint_c.tolerance_below = 0.01 joint_c.weight = 1.0 goal_c.joint_constraints.append(joint_c) userdata.move_it_joint_goal.request.goal_constraints.append(goal_c) userdata.move_it_joint_goal.request.num_planning_attempts = 5 userdata.move_it_joint_goal.request.allowed_planning_time = 5.0 userdata.move_it_joint_goal.planning_options.plan_only = False # False = Plan + Execute ; True = Plan only userdata.move_it_joint_goal.planning_options.planning_scene_diff.is_diff = True userdata.move_it_joint_goal.request.group_name = userdata.manip_joint_group rospy.loginfo("Group Name: " + userdata.manip_joint_group) rospy.loginfo("Joints name: " + str(userdata.manip_joint_names)) rospy.loginfo("Joints Values: " + str(userdata.manip_goal_joint_pose)) rospy.loginfo("GOAL TO SEND IS:... " + str(userdata.move_it_joint_goal)) return "succeeded"
def init_constraint(): joint_constraint = JointConstraint() joint_constraint.joint_name = "shoulder_pan_joint" joint_constraint.position = 0 joint_constraint.tolerance_above = 0.7854 joint_constraint.tolerance_below = 0.7854 joint_constraint.weight = 1 base_constraint.joint_constraints.append(joint_constraint)
def _do_ik(self, px, py, pz, arm): """ Does inverse kinematics for a given arm and given position. This solver is collision aware. Args: px, py, pz (float): Cartesian coordinates for position of arm end effector. arm (str): Which arm ("right_arm" or "left_arm") to do ik for. Returns: list: List of joint configuration for specific arm. Returns an empty list if unable to do IK. """ IK_INFO_NAME = "/pr2_%s_kinematics/get_ik_solver_info" % (arm) IK_NAME = "/compute_ik" ik_solver_info_service_proxy = rospy.ServiceProxy(IK_INFO_NAME, GetKinematicSolverInfo) ik_info_req = GetKinematicSolverInfoRequest() rospy.loginfo("LightningTest: do_ik: Waiting for %s" % (IK_INFO_NAME)) rospy.wait_for_service(IK_INFO_NAME) rospy.loginfo("About to get IK info.") ik_info_res = ik_solver_info_service_proxy(ik_info_req) rospy.loginfo("Got IK Solver info.") ik_solver_service_proxy = rospy.ServiceProxy(IK_NAME, GetPositionIK) ik_solve_req = GetPositionIKRequest() ik_solve_req.ik_request.group_name = arm ik_solve_req.ik_request.timeout = rospy.Duration(5.0) ik_solve_req.ik_request.ik_link_name = "%s_wrist_roll_link" % (arm[0]) ik_solve_req.ik_request.pose_stamped.header.frame_id = "odom_combined" ik_solve_req.ik_request.pose_stamped.pose.position.x = px ik_solve_req.ik_request.pose_stamped.pose.position.y = py ik_solve_req.ik_request.pose_stamped.pose.position.z = pz ik_solve_req.ik_request.pose_stamped.pose.orientation.x = 0.0; ik_solve_req.ik_request.pose_stamped.pose.orientation.y = 0.0; ik_solve_req.ik_request.pose_stamped.pose.orientation.z = 0.0; ik_solve_req.ik_request.pose_stamped.pose.orientation.w = 1.0; # TODO: Consider retrieving current robto state. ik_solve_req.ik_request.robot_state.joint_state.name = ik_info_res.kinematic_solver_info.joint_names; for i in xrange(len(ik_info_res.kinematic_solver_info.joint_names)): joint_limits = ik_info_res.kinematic_solver_info.limits[i] ik_solve_req.ik_request.robot_state.joint_state.position.append((joint_limits.min_position + joint_limits.max_position)/2.0) joint_constraint = JointConstraint() joint_constraint.joint_name = ik_info_res.kinematic_solver_info.joint_names[i] joint_constraint.position = (joint_limits.max_position + joint_limits.min_position) / 2.0 joint_constraint.tolerance_above = (joint_limits.max_position - joint_limits.min_position) / 2.0 joint_constraint.tolerance_below = joint_constraint.tolerance_above ik_solve_req.ik_request.constraints.joint_constraints.append(joint_constraint) rospy.loginfo("LightningTest: do_ik: Waiting for IK service.") rospy.wait_for_service(IK_NAME) ik_solve_res = ik_solver_service_proxy(ik_solve_req) if ik_solve_res.error_code.val == ik_solve_res.error_code.SUCCESS: return ik_solve_res.solution.joint_state.position[30:37] else: rospy.loginfo("Lightning tester: cannot move to sampled position...trying another position") return []
def plan_trajectory(self, start_point, goal_point, planner_number, joint_names, group_name, planning_time, planner_config_name, plan_type='pfs'): """ Given a start and goal point, plan by classical planner. Args: start_point (list of float): A starting joint configuration. goal_point (list of float): A goal joint configuration. planner_number (int): The index of the planner to be used as returned by acquire_planner(). joint_names (list of str): The name of the joints corresponding to start_point and goal_point. group_name (str): The name of the group to which the joint names correspond. planning_time (float): Maximum allowed time for planning, in seconds. planner_config_name (str): Type of planner to use. plan_type (str): either pfs or rr. If rr we don't use the path length threshold. Return: list of list of float: A sequence of points representing the joint configurations at each point on the path. """ planner_client = rospy.ServiceProxy(self.planners[planner_number], GetMotionPlan) rospy.loginfo("%s Plan Trajectory Wrapper: got a plan_trajectory request for %s with start = %s and goal = %s" \ % (rospy.get_name(), self.planners[planner_number], start_point, goal_point)) # Put together the service request. req = GetMotionPlanRequest() req.motion_plan_request.workspace_parameters.header.stamp = rospy.get_rostime() req.motion_plan_request.group_name = group_name req.motion_plan_request.num_planning_attempts = 1 req.motion_plan_request.allowed_planning_time = planning_time req.motion_plan_request.planner_id = planner_config_name #using RRT planner by default req.motion_plan_request.start_state.joint_state.header.stamp = rospy.get_rostime() req.motion_plan_request.start_state.joint_state.name = joint_names req.motion_plan_request.start_state.joint_state.position = start_point req.motion_plan_request.goal_constraints.append(Constraints()) req.motion_plan_request.goal_constraints[0].joint_constraints = [] for i in xrange(len(joint_names)): temp_constraint = JointConstraint() temp_constraint.joint_name = joint_names[i] temp_constraint.position = goal_point[i] temp_constraint.tolerance_above = 0.05; temp_constraint.tolerance_below = 0.05; req.motion_plan_request.goal_constraints[0].joint_constraints.append(temp_constraint) #call the planner rospy.wait_for_service(self.planners[planner_number]) rospy.loginfo("Plan Trajectory Wrapper: sent request to service %s" % planner_client.resolved_name) plan_time = np.inf try: plan_time = time.time() response = planner_client(req) plan_time = time.time() - plan_time except rospy.ServiceException, e: rospy.loginfo("%s Plan Trajectory Wrapper: service call failed: %s" % (rospy.get_name(), e)) return plan_time, None
def set_path_constraints(self, value): # In order to force planning in joint coordinates we add dummy joint # constrains. empty_joint_constraints = JointConstraint() empty_joint_constraints.joint_name = "r1_joint_grip" empty_joint_constraints.position = 0 empty_joint_constraints.tolerance_above = 1.5 empty_joint_constraints.tolerance_below = 1.5 empty_joint_constraints.weight = 1 value.joint_constraints.append(empty_joint_constraints) MoveGroupCommander.set_path_constraints(self, value)
def plan_execute_pose_constraints_cb(self, goal): feedback = PlanExecutePoseConstraintsFeedback() result = PlanExecutePoseConstraintsResult() result.result = True #include default arm_base_joint // it is bug that moveit does not exclude passive joint in thier joint trajectory. js_base = JointConstraint() js_base.joint_name = "arm_base_joint" js_base.position = 0.0 js_base.tolerance_above = 0.001 js_base.tolerance_below = 0.001 js_base.weight = 1.0 self.contraints.name = "constraints" self.contraints.joint_constraints.append(js_base) print("==================== Joint Constraints ====================") for js in goal.joint_constraints: self.contraints.joint_constraints.append(js) print(js) self.group.set_path_constraints(self.contraints) print("============================================================") self.group.clear_pose_targets() self.group.set_start_state_to_current_state() try: self.group.set_pose_target(goal.target_pose) except MoveItCommanderException: result.result = False return rospy.loginfo('Planning goal pose...') plan1 = self.group.plan() if len(plan1.joint_trajectory.points) == 0: result.result = False return display_trajectory = moveit_msgs.msg.DisplayTrajectory() display_trajectory.trajectory_start = self.robot.get_current_state() display_trajectory.trajectory.append(plan1) self.traj_publisher.publish(display_trajectory) # rospy.sleep(0.5) rospy.loginfo('Start moving...') self.group.go(wait=True) rospy.loginfo('End moving') # rospy.sleep(2.0) self.group.set_path_constraints(None) self.contraints.joint_constraints = [] rospy.loginfo('Planning goal pose succeeded.') self.action_plan_execute_pose_w_constraints.set_succeeded(result)
def build_joint_constraints(joint_state): joint_constraints = [] for i, joint_angle in enumerate(joint_state): jc = JointConstraint() jc.joint_name = self.JOINTS[i] jc.position = joint_angle jc.tolerance_above = np.radians( self.MAX_LINEAR_JOINT_FLUCTUATION) jc.tolerance_below = np.radians( self.MAX_LINEAR_JOINT_FLUCTUATION) jc.weight = 1.0 joint_constraints.append(jc) return joint_constraints
def setJointConstraints(goal_config, group_handle): constraints = []; names = group_handle.get_joints(); for num in range(0,len(names)): jnt_constraint = JointConstraint(); jnt_constraint.joint_name = names[num]; jnt_constraint.position = goal_config[num]; jnt_constraint.tolerance_above = 0.0001; jnt_constraint.tolerance_below = 0.0001; jnt_constraint.weight = 1.0; constraints.append(jnt_constraint); return constraints;
def plan_trajectory(self, start_point, goal_point, planner_number, joint_names, group_name, planning_time, planner_config_name): """ Given a start and goal point, returns the planned path. Args: start_point (list of float): A starting joint configuration. goal_point (list of float): A goal joint configuration. planner_number (int): The index of the planner to be used as returned by acquire_planner(). joint_names (list of str): The name of the joints corresponding to start_point and goal_point. group_name (str): The name of the group to which the joint names correspond. planning_time (float): Maximum allowed time for planning, in seconds. planner_config_name (str): Type of planner to use. Return: list of list of float: A sequence of points representing the joint configurations at each point on the path. """ planner_client = rospy.ServiceProxy(self.planners[planner_number], GetMotionPlan) rospy.loginfo("Plan Trajectory Wrapper: got a plan_trajectory request for %s with start = %s and goal = %s" % (self.planners[planner_number], start_point, goal_point)) # Put together the service request. req = GetMotionPlanRequest() req.motion_plan_request.workspace_parameters.header.stamp = rospy.get_rostime() req.motion_plan_request.group_name = group_name req.motion_plan_request.num_planning_attempts = 1 req.motion_plan_request.allowed_planning_time = planning_time req.motion_plan_request.planner_id = planner_config_name #using RRT planner by default req.motion_plan_request.start_state.joint_state.header.stamp = rospy.get_rostime() req.motion_plan_request.start_state.joint_state.name = joint_names req.motion_plan_request.start_state.joint_state.position = start_point req.motion_plan_request.goal_constraints.append(Constraints()) req.motion_plan_request.goal_constraints[0].joint_constraints = [] for i in xrange(len(joint_names)): temp_constraint = JointConstraint() temp_constraint.joint_name = joint_names[i] temp_constraint.position = goal_point[i] temp_constraint.tolerance_above = 0.05; temp_constraint.tolerance_below = 0.05; req.motion_plan_request.goal_constraints[0].joint_constraints.append(temp_constraint) #call the planner rospy.wait_for_service(self.planners[planner_number]) rospy.loginfo("Plan Trajectory Wrapper: sent request to service %s" % planner_client.resolved_name) try: response = planner_client(req) except rospy.ServiceException, e: rospy.loginfo("Plan Trajectory Wrapper: service call failed: %s"%e) return None
def createJointConstraints(pose_from_params, tolerances=0.1): """Create a JointConstraints message with its joint names and positions with some default tolerances @param pose_from_params dictionary with names of the joints and it's values @param tolerances the tolerance in radians for the joint positions, defaults to 0.1 @return moveit_msgs/JointConstraints[] message with the joint names and positions""" joint_constraints = [] for joint in pose_from_params: joint_constraint = JointConstraint() joint_constraint.joint_name = joint joint_constraint.position = pose_from_params[joint] joint_constraint.tolerance_above = tolerances joint_constraint.tolerance_below = tolerances joint_constraint.weight = 1.0 joint_constraints.append(joint_constraint) return joint_constraints
def init_path_constraints(joint_no, upper_limit, lower_limit, weight): joint_constraint = JointConstraint() UpperArmPath.path_constraints.name = UpperArmPath.__GROUP_NAME upper_limit = upper_limit / 180.0 * math.pi lower_limit = lower_limit / 180.0 * math.pi joint_constraint.position = (upper_limit + lower_limit) / 2.0 joint_constraint.tolerance_above = (upper_limit - lower_limit) / 2.0 joint_constraint.tolerance_below = (upper_limit - lower_limit) / 2.0 joint_constraint.weight = weight joint_constraint.joint_name = "upper_joint%d" % (joint_no) UpperArmPath.path_constraints.joint_constraints.append( joint_constraint)
def createJointConstraintsZero(): joint_positions = [1.7567944054, 1.68571255762, -0.35731008621, 1.06480870567, 1.36986326531, -0.662985424101, -1.31376998814] joint_constraints = [] for joint_num in range(1,8): joint_constraint = JointConstraint() joint_constraint.joint_name = 'arm_right_' + str(joint_num) + '_joint' joint_constraint.tolerance_above = 0.1 joint_constraint.tolerance_below = 0.1 joint_constraint.weight = 1.0 joint_constraint.position = 0.0 joint_constraints.append(joint_constraint) print str(joint_constraint) return joint_constraints
def createJointConstraintsZero(): joint_positions = [ 1.7567944054, 1.68571255762, -0.35731008621, 1.06480870567, 1.36986326531, -0.662985424101, -1.31376998814 ] joint_constraints = [] for joint_num in range(1, 8): joint_constraint = JointConstraint() joint_constraint.joint_name = 'arm_right_' + str(joint_num) + '_joint' joint_constraint.tolerance_above = 0.1 joint_constraint.tolerance_below = 0.1 joint_constraint.weight = 1.0 joint_constraint.position = 0.0 joint_constraints.append(joint_constraint) print str(joint_constraint) return joint_constraints
def createJointConstraints(joint_names, joint_positions, tolerances=0.1): """Create a JointConstraints message with its joint names and positions with some default tolerances @param joint_names names of the joints which will reference to values in param joint_positions @param joint_positions values for the joints specified in joint_names @param tolerances the tolerance in radians for the joint positions, defaults to 0.1 @return moveit_msgs/JointConstraints[] message with the joint names and positions""" joint_constraints = [] for joint, pos in zip(joint_names, joint_positions): joint_constraint = JointConstraint() joint_constraint.joint_name = joint joint_constraint.position = pos joint_constraint.tolerance_above = tolerances joint_constraint.tolerance_below = tolerances joint_constraint.weight = 1.0 joint_constraints.append(joint_constraint) return joint_constraints
def plan_execute_named_pose_cb(self, goal): feedback = PlanExecuteNamedPoseFeedback() result = PlanExecuteNamedPoseResult() result.result = True self.group.clear_pose_targets() #self.group.set_start_state_to_current_state() #include default arm_base_joint // it is bug that moveit does not exclude passive joint in thier joint trajectory. js_base = JointConstraint() js_base.joint_name = "arm_base_joint" js_base.position = 0.0 js_base.tolerance_above = 0.001 js_base.tolerance_below = 0.001 js_base.weight = 1.0 self.contraints.name = "constraints" self.contraints.joint_constraints.append(js_base) self.group.set_path_constraints(self.contraints) try: self.group.set_named_target(goal.target_name) except MoveItCommanderException: result.result = False self.action_plan_execute_named_pose.set_succeeded(result) return rospy.loginfo('Planning named [%s] pose...' % goal.target_name) plan1 = self.group.plan() # print("plan1 type : ", type(plan1)) # print("plan : ", plan1) display_trajectory = moveit_msgs.msg.DisplayTrajectory() display_trajectory.trajectory_start = self.robot.get_current_state() display_trajectory.trajectory.append(plan1) self.traj_publisher.publish(display_trajectory) # rospy.sleep(0.5) rospy.loginfo('Start moving...') self.group.go(wait=True) rospy.loginfo('Planning named pose succeeded.') self.action_plan_execute_named_pose.set_succeeded(result)
def compute_ik(self, pose): if isinstance(pose, rox.Transform): pose = rox_msg.transform2pose_stamped_msg(pose) assert isinstance(pose, PoseStamped) if pose.header.frame_id is None or pose.header.frame_id == "": pose.header.frame_id = "world" get_planning_scene_req = GetPlanningSceneRequest() get_planning_scene_req.components.components = PlanningSceneComponents.ROBOT_STATE get_planning_scene_res = self._get_planning_scene( get_planning_scene_req) robot_state_start = get_planning_scene_res.scene.robot_state ik_request = PositionIKRequest() ik_request.group_name = self.moveit_group.get_name() ik_request.robot_state = robot_state_start ik_request.avoid_collisions = True ik_request.timeout = rospy.Duration(30.0) ik_request.pose_stamped = pose ik_request.ik_link_name = self.moveit_group.get_end_effector_link() for i in xrange(len(robot_state_start.joint_state.name)): j = JointConstraint() j.joint_name = robot_state_start.joint_state.name[i] j.position = robot_state_start.joint_state.position[i] j.tolerance_above = np.deg2rad(170) j.tolerance_below = np.deg2rad(170) j.weight = 100 ik_request.constraints.joint_constraints.append(j) ik_request_req = GetPositionIKRequest(ik_request) ik_request_res = self._compute_ik(ik_request_req) if (ik_request_res.error_code.val != MoveItErrorCodes.SUCCESS): raise Exception("Could not compute inverse kinematics") return ik_request_res.solution.joint_state.position
def plan_between_states(group_name, joint_state_A, joint_state_B, robot_state=None): """ Plan trajectory between two joint states. """ # Create robot state robot_state = joint_to_robot_state(joint_state_A, robot_state) # Prepare motion plan request req = GetMotionPlanRequest() req.motion_plan_request.start_state = robot_state req.motion_plan_request.group_name = group_name req.motion_plan_request.planner_id = "RRTConnectkConfigDefault" constraints = Constraints() constraints.name = "goal" for name, pos in zip(joint_state_B.name, joint_state_B.position): joint_constraints = JointConstraint() joint_constraints.joint_name = name joint_constraints.position = pos joint_constraints.tolerance_above = 0.001 joint_constraints.tolerance_below = 0.001 joint_constraints.weight = 100 constraints.joint_constraints.append(joint_constraints) req.motion_plan_request.goal_constraints.append(constraints) # Call planner planner_srv = rospy.ServiceProxy('/plan_kinematic_path', GetMotionPlan) try: res = planner_srv(req) except rospy.service.ServiceException, e: rospy.logerr("Service 'plan_kinematic_path' call failed: %s", str(e)) return None
def set_joint_goal(self, joint_positions, tolerance=0.001, weight=1.0, joint_names=None): """ Set goal position in joint space. With `joint_names` specified, `joint_positions` can be defined for specific joints. Otherwise, first `n` joints defined in `init_robot()` will be used, where `n` is the length of `joint_positions`. """ if joint_names == None: joint_names = self.arm_joints for i in range(len(joint_positions)): joint_constraint = JointConstraint() joint_constraint.joint_name = joint_names[i] joint_constraint.position = joint_positions[i] joint_constraint.tolerance_above = tolerance joint_constraint.tolerance_below = tolerance joint_constraint.weight = weight (self.kinematic_path_request.motion_plan_request. goal_constraints[-1].joint_constraints.append(joint_constraint))
def create_basic_mp_joint_request(joint_names, joint_values, planner_id): motion_plan_request = MotionPlanRequest() motion_plan_request.group_name = move_group motion_plan_request.num_planning_attempts = 1 motion_plan_request.allowed_planning_time = 5.0 motion_plan_request.workspace_parameters = WorkspaceParameters() motion_plan_request.max_velocity_scaling_factor = 0.5 motion_plan_request.max_acceleration_scaling_factor = 0.5 motion_plan_request.planner_id = planner_id joint_constraints = [] for i in range(len(joint_names)): joint_con = JointConstraint() joint_con.joint_name = joint_names[i] joint_con.position = joint_values[i] joint_con.tolerance_above = 0.0001 joint_con.tolerance_below = 0.0001 joint_con.weight = 0.0001 joint_constraints.append(joint_con) constraint = Constraints() constraint.joint_constraints = joint_constraints constraints = [constraint] motion_plan_request.goal_constraints = constraints return motion_plan_request
def move_to_state(self, joint_states): if self.verbose: print 'Moving to next state' mpr = MotionPlanRequest() con = Constraints() for name, val in joint_states.items(): if name not in self.joints_to_exclude: jc = JointConstraint() jc.joint_name = name jc.position = val jc.tolerance_above = self.joint_angle_tolerance jc.tolerance_below = self.joint_angle_tolerance jc.weight = 1.0 con.joint_constraints.append(jc) mpr.goal_constraints = [con] mpr.group_name = self.planning_group_name mpr.allowed_planning_time = 3.0 # [s] # print mpr try: req = rospy.ServiceProxy('/plan_kinematic_path', GetMotionPlan) res = req(mpr) traj = res.motion_plan_response.trajectory except rospy.ServiceException, e: print "Service call failed: %s" % e
def kinect_planner(): global fresh_data global joint_target, pose_target, goal global joints_req, pose_req moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('kinect_trajectory_planner', anonymous=True) rate = rospy.Rate(0.5) #rospy.sleep(3) # Instantiate a RobotCommander object. robot = moveit_commander.RobotCommander() # Instantiate a PlanningSceneInterface object (interface to the world surrounding the robot). scene = moveit_commander.PlanningSceneInterface() print "Remember to disable firewall if it's not working" # Instantiate MoveGroupCommander objects for arms and Kinect2. group = moveit_commander.MoveGroupCommander("Kinect2_Target") group_left_arm = moveit_commander.MoveGroupCommander("left_arm") group_right_arm = moveit_commander.MoveGroupCommander("right_arm") # We create this DisplayTrajectory publisher to publish # trajectories for RVIZ to visualize. display_trajectory_publisher = rospy.Publisher('planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=5) # Set the planner for Moveit group.set_planner_id("RRTConnectkConfigDefault") group.set_planning_time(8) group.set_pose_reference_frame('base_footprint') # Setting tolerance group.set_goal_tolerance(0.08) group.set_num_planning_attempts(10) # Suscribing to the desired pose topic target = rospy.Subscriber("desired_pose", PoseStamped, callback) target_joint = rospy.Subscriber("desired_joints", pose_w_joints, callback_joints) # Locating the arms and the Kinect2 sensor group_left_arm_values = group_left_arm.get_current_joint_values() group_right_arm_values = group_right_arm.get_current_joint_values() #group_kinect_values = group_kinect.get_current_joint_values() neck_init_joints = group.get_current_joint_values() neck_init_joints[0] = -1.346 neck_init_joints[1] = -1.116 neck_init_joints[2] = -2.121 neck_init_joints[3] = 0.830 neck_init_joints[4] = 1.490 neck_init_joints[5] = 0.050 neck_init_joints[6] = 0 neck_init_joints[7] = 0 neck_init_joints[8] = 0 neck_init_joints[9] = 0 # Creating a box to limit the arm position box_pose = PoseStamped() box_pose.pose.orientation.w = 1 box_pose.pose.position.x = 0.6 box_pose.pose.position.y = 0.03 box_pose.pose.position.z = 1.5 box_pose.header.frame_id = 'base_footprint' scene.add_box('box1', box_pose, (0.4, 0.4, 0.1)) rospy.sleep(2) # Defining position constraints for the trajectory of the kinect neck_const = Constraints() neck_const.name = 'neck_constraints' target_const = JointConstraint() target_const.joint_name = "neck_joint_end" target_const.position = 0.95 target_const.tolerance_above = 0.45 target_const.tolerance_below = 0.05 target_const.weight = 0.9 # Importance of this constraint neck_const.joint_constraints.append(target_const) # Talking to the robot client = actionlib.SimpleActionClient('/Kinect2_Target_controller/follow_joint_trajectory', FollowJointTrajectoryAction) print "====== Waiting for server..." client.wait_for_server() print "====== Connected to server" while not rospy.is_shutdown(): if fresh_data == True: #If a new pose is received, plan. # Update arms position group_left_arm_values = group_left_arm.get_current_joint_values() group_right_arm_values = group_right_arm.get_current_joint_values() # Set the target pose (or joint values) and generate a plan if pose_req: group.set_pose_target(pose_target) if joints_req: group.set_joint_value_target(joint_target) print "=========== Calculating trajectory... \n" # Generate several plans and compare them to get the shorter one plan_opt = dict() differ = dict() try: num = 1 rep = 0 # Generate several plans and compare them to get the shortest one #for num in range(1,8): while num < 7: num += 1 plan_temp = group.plan() move(plan_temp) plan_opt[num] = goal.trajectory diff=0 for point in goal.trajectory.points: # calculate the distance between initial pose and planned one for i in range(0,6): diff = abs(neck_init_joints[i] - point.positions[i])+abs(diff) differ[num] = diff # If the current plan is good, take it if diff < 110: break # If plan is too bad, don't consider it if diff > 400: num = num - 1 print "Plan is too long. Replanning." rep = rep + 1 if rep > 4: num = num +1 # If no plan was found... if differ == {}: print "---- Fail: No motion plan found. No execution attempted. Probably robot joints are out of range." break else: # Select the shortest plan min_plan = min(differ.itervalues()) select = [k for k, value in differ.iteritems() if value == min_plan] goal.trajectory = plan_opt[select[0]] #print " Plan difference:======= ", differ #print " Selected plan:========= ", select[0] # Remove the last 4 names and data from each point (dummy joints) before sending the goal goal.trajectory.joint_names = goal.trajectory.joint_names[:6] for point in goal.trajectory.points: point.positions = point.positions[:6] point.velocities = point.velocities[:6] point.accelerations = point.accelerations[:6] print "Sending goal" client.send_goal(goal) print "Waiting for result" client.wait_for_result() # Change the position of the virtual joint to avoid collision neck_joints = group.get_current_joint_values() #neck_joints[6] = 0.7 #group.set_joint_value_target(neck_joints) #group.go(wait=True) except (KeyboardInterrupt, SystemExit): client.cancel_goal() raise rate.sleep() fresh_data = False rate.sleep()
def right_arm_go_to_pose_goal(self): # 设置动作对象变量,此处为arm right_arm = self.right_arm # 获取当前末端执行器位置姿态 pose_goal = right_arm.get_current_pose().pose # 限制末端夹具运动 right_joint_const = JointConstraint() right_joint_const.joint_name = "gripper_r_joint_r" if Rightfinger > -55: right_joint_const.position = 0.024 else: right_joint_const.position = 0 right_joint_const.weight = 1.0 # 限制1轴转动 right_joint1_const = JointConstraint() right_joint1_const.joint_name = "yumi_joint_1_r" right_joint1_const.position = 0 right_joint1_const.tolerance_above = 120 right_joint1_const.tolerance_below = 0 right_joint1_const.weight = 1.0 # 限制2轴转动 right_joint2_const = JointConstraint() right_joint2_const.joint_name = "yumi_joint_2_r" right_joint2_const.position = 0 right_joint2_const.tolerance_above = 0 right_joint2_const.tolerance_below = 150 right_joint2_const.weight = 1.0 # 限制3轴转动 right_joint3_const = JointConstraint() right_joint3_const.joint_name = "yumi_joint_3_r" right_joint3_const.position = 0 right_joint3_const.tolerance_above = 35 right_joint3_const.tolerance_below = 55 right_joint3_const.weight = 1.0 # 限制4轴转动 right_joint4_const = JointConstraint() right_joint4_const.joint_name = "yumi_joint_4_r" right_joint4_const.position = 0 right_joint4_const.tolerance_above = 60 right_joint4_const.tolerance_below = 75 right_joint4_const.weight = 1.0 # 限制5轴转动 right_joint5_const = JointConstraint() right_joint5_const.joint_name = "yumi_joint_5_r" right_joint5_const.position = 40 right_joint5_const.tolerance_above = 50 right_joint5_const.tolerance_below = 20 right_joint5_const.weight = 1.0 # 限制6轴转动 right_joint6_const = JointConstraint() right_joint6_const.joint_name = "yumi_joint_6_r" right_joint6_const.position = 0 right_joint6_const.tolerance_above = 10 right_joint6_const.tolerance_below = 35 right_joint6_const.weight = 1.0 # 限制7轴转动 right_joint7_const = JointConstraint() right_joint7_const.joint_name = "yumi_joint_7_r" right_joint7_const.position = -10 right_joint7_const.tolerance_above = 0 right_joint7_const.tolerance_below = 160 right_joint7_const.weight = 1.0 # 限制末端位移 right_position_const = PositionConstraint() right_position_const.header = Header() right_position_const.link_name = "gripper_r_joint_r" right_position_const.target_point_offset.x = 0.5 right_position_const.target_point_offset.y = -0.5 right_position_const.target_point_offset.z = 1.0 right_position_const.weight = 1.0 # 添加末端姿态约束: right_orientation_const = OrientationConstraint() right_orientation_const.header = Header() right_orientation_const.orientation = pose_goal.orientation right_orientation_const.link_name = "gripper_r_finger_r" right_orientation_const.absolute_x_axis_tolerance = 0.50 right_orientation_const.absolute_y_axis_tolerance = 0.25 right_orientation_const.absolute_z_axis_tolerance = 0.50 right_orientation_const.weight = 100 # 施加全约束 consts = Constraints() consts.joint_constraints = [right_joint_const] # consts.orientation_constraints = [right_orientation_const] # consts.position_constraints = [right_position_const] right_arm.set_path_constraints(consts) # 设置动作对象目标位置姿态 pose_goal.orientation.x = Right_Qux pose_goal.orientation.y = Right_Quy pose_goal.orientation.z = Right_Quz pose_goal.orientation.w = Right_Quw pose_goal.position.x = (Neurondata[5] - 0.05) * 1.48 + 0.053 pose_goal.position.y = (Neurondata[3] + 0.18) * 1.48 - 0.12 pose_goal.position.z = (Neurondata[4] - 0.53) * 1.48 + 0.47 right_arm.set_pose_target(pose_goal) print "End effector pose %s" % pose_goal # # 设置动作对象目标位置姿态 # pose_goal.orientation.x = 0.1644 # pose_goal.orientation.y = 0.3111 # pose_goal.orientation.z = 0.9086 # pose_goal.orientation.w = 0.2247 # pose_goal.position.x = (Neurondata[5]-0.05)*1.48+0.053 # pose_goal.position.y = (Neurondata[3]+0.18)*1.48-0.12 # pose_goal.position.z = (Neurondata[4]-0.53)*1.48+0.47 # right_arm.set_pose_target(pose_goal) # print "End effector pose %s" % pose_goal # # 设置动作对象目标位置姿态 # pose_goal.orientation.x = pose_goal.orientation.x # pose_goal.orientation.y = pose_goal.orientation.y # pose_goal.orientation.z = pose_goal.orientation.z # pose_goal.orientation.w = pose_goal.orientation.w # pose_goal.position.x = pose_goal.position.x # pose_goal.position.y = pose_goal.position.y - 0.01 # pose_goal.position.z = pose_goal.position.z # right_arm.set_pose_target(pose_goal) # print "End effector pose %s" % pose_goal # 规划和输出动作 traj = right_arm.plan() right_arm.execute(traj, wait=False) # 动作完成后清除目标信息 right_arm.clear_pose_targets() # 清除路径约束 right_arm.clear_path_constraints() # 确保没有剩余未完成动作在执行 right_arm.stop()
def left_arm_go_to_pose_goal(self): # 设置动作对象变量,此处为arm left_arm = self.left_arm # 获取当前末端执行器位置姿态 pose_goal = left_arm.get_current_pose().pose # 限制末端夹具运动 left_joint_const = JointConstraint() left_joint_const.joint_name = "gripper_l_joint_r" if Leftfinger > -32: left_joint_const.position = 0.024 else: left_joint_const.position = 0 left_joint_const.weight = 1.0 # 限制末端位移 left_position_const = PositionConstraint() left_position_const.header = Header() left_position_const.link_name = "gripper_l_joint_r" left_position_const.target_point_offset.x = 0.5 left_position_const.target_point_offset.y = -0.5 left_position_const.target_point_offset.z = 1.0 left_position_const.weight = 1.0 # # 限制1轴转动 left_joint1_const = JointConstraint() left_joint1_const.joint_name = "yumi_joint_1_l" left_joint1_const.position = 0 left_joint1_const.tolerance_above = 1.76 left_joint1_const.tolerance_below = 0 left_position_const.weight = 1.0 # 限制2轴转动 left_joint2_const = JointConstraint() left_joint2_const.joint_name = "yumi_joint_2_l" left_joint2_const.position = 0 left_joint2_const.tolerance_above = 0 left_joint2_const.tolerance_below = 150 left_joint2_const.weight = 1.0 # 限制3轴转动 left_joint3_const = JointConstraint() left_joint3_const.joint_name = "yumi_joint_3_l" left_joint3_const.position = 0 left_joint3_const.tolerance_above = 35 left_joint3_const.tolerance_below = 55 left_joint3_const.weight = 1.0 # 限制4轴转动 left_joint4_const = JointConstraint() left_joint4_const.joint_name = "yumi_joint_4_l" left_joint4_const.position = 0 left_joint4_const.tolerance_above = 60 left_joint4_const.tolerance_below = 75 left_joint4_const.weight = 1.0 # 限制5轴转动 right_joint5_const = JointConstraint() right_joint5_const.joint_name = "yumi_joint_5_l" right_joint5_const.position = 40 right_joint5_const.tolerance_above = 50 right_joint5_const.tolerance_below = 20 right_joint5_const.weight = 1.0 # 限制6轴转动 left_joint6_const = JointConstraint() left_joint6_const.joint_name = "yumi_joint_6_l" left_joint6_const.position = 0 left_joint6_const.tolerance_above = 10 left_joint6_const.tolerance_below = 35 left_joint6_const.weight = 1.0 # 限制7轴转动 left_joint7_const = JointConstraint() left_joint7_const.joint_name = "yumi_joint_7_l" left_joint7_const.position = -10 left_joint7_const.tolerance_above = 0 left_joint7_const.tolerance_below = 160 left_joint7_const.weight = 1.0 # 限制末端位移 left_position_const = PositionConstraint() left_position_const.header = Header() left_position_const.link_name = "gripper_l_joint_r" left_position_const.target_point_offset.x = 0.5 left_position_const.target_point_offset.y = 0.25 left_position_const.target_point_offset.z = 0.5 left_position_const.weight = 1.0 # 添加末端姿态约束: left_orientation_const = OrientationConstraint() left_orientation_const.header = Header() left_orientation_const.orientation = pose_goal.orientation left_orientation_const.link_name = "gripper_l_finger_r" left_orientation_const.absolute_x_axis_tolerance = 0.5 left_orientation_const.absolute_y_axis_tolerance = 0.25 left_orientation_const.absolute_z_axis_tolerance = 0.5 left_orientation_const.weight = 1 # 施加全约束 consts = Constraints() consts.joint_constraints = [left_joint_const] # consts.orientation_constraints = [left_orientation_const] # consts.position_constraints = [left_position_const] left_arm.set_path_constraints(consts) # 设置动作对象目标位置姿态 pose_goal.orientation.x = Left_Qux pose_goal.orientation.y = Left_Quy pose_goal.orientation.z = Left_Quz pose_goal.orientation.w = Left_Quw pose_goal.position.x = (Neurondata[11] - 0.05) * 1.48 + 0.053 pose_goal.position.y = (Neurondata[9] - 0.18) * 1.48 + 0.12 pose_goal.position.z = (Neurondata[10] - 0.53) * 1.48 + 0.47 left_arm.set_pose_target(pose_goal) print "End effector pose %s" % pose_goal # # 设置动作对象目标位置姿态 # pose_goal.orientation.x = pose_goal.orientation.x # pose_goal.orientation.y = pose_goal.orientation.y # pose_goal.orientation.z = pose_goal.orientation.z # pose_goal.orientation.w = pose_goal.orientation.w # pose_goal.position.x = pose_goal.position.x # pose_goal.position.y = pose_goal.position.y - 0.01 # pose_goal.position.z = pose_goal.position.z # left_arm.set_pose_target(pose_goal) # print "End effector pose %s" % pose_goal # 规划和输出动作 traj = left_arm.plan() left_arm.execute(traj, wait=False) # 动作完成后清除目标信息 left_arm.clear_pose_targets() # 清除路径约束 left_arm.clear_path_constraints() # 确保没有剩余未完成动作在执行 left_arm.stop()
def main(): """ Main Script """ #=================================================== # Code to add gripper # rospy.init_node('gripper_test') # # Set up the right gripper # right_gripper = robot_gripper.Gripper('right_gripper') # #Calibrate the gripper (other commands won't work unless you do this first) # print('Calibrating...') # right_gripper.calibrate() # rospy.sleep(2.0) # #Close the right gripper to hold publisher # # MIGHT NOT NEED THIS # print('Closing...') # right_gripper.close() # rospy.sleep(1.0) #=================================================== planner = PathPlanner("right_arm") Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5 ]) # Stolen from 106B Students Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned limb = intera_interface.Limb("right") control = Controller(Kp, Kd, Ki, Kw, limb) ## ## Add the obstacle to the planning scene here ## pose = PoseStamped() pose.header.stamp = rospy.Time.now() pose.header.frame_id = "base" pose.pose.position.x = -1.1 pose.pose.position.y = 0 pose.pose.position.z = 0 pose.pose.orientation.x = 0 pose.pose.orientation.y = 0 pose.pose.orientation.z = 0 pose.pose.orientation.w = 1 planner.add_box_obstacle([1, 1, 5], "left_side_wall", pose) pose = PoseStamped() pose.header.stamp = rospy.Time.now() pose.header.frame_id = "base" pose.pose.position.x = 0 pose.pose.position.y = -1.0 pose.pose.position.z = 0 pose.pose.orientation.x = 0 pose.pose.orientation.y = 0 pose.pose.orientation.z = 0 pose.pose.orientation.w = 1 planner.add_box_obstacle([1, 1, 5], "right_side_wall", pose) pose = PoseStamped() pose.header.stamp = rospy.Time.now() pose.header.frame_id = "base" pose.pose.position.x = 0 pose.pose.position.y = 1.1 pose.pose.position.z = 0 pose.pose.orientation.x = 0 pose.pose.orientation.y = 0 pose.pose.orientation.z = 0 pose.pose.orientation.w = 1 planner.add_box_obstacle([1, 1, 5], "back_wall", pose) orien_const = OrientationConstraint() orien_const.link_name = "right_gripper" orien_const.header.frame_id = "ar_marker_1" #orien_const.orientation.y = np.pi/2 #orien_const.orientation.w = 1; orien_const.absolute_x_axis_tolerance = 1 orien_const.absolute_y_axis_tolerance = 1 orien_const.absolute_z_axis_tolerance = 1 orien_const.weight = .5 joint_const = JointConstraint() # Constrain the position of a joint to be within a certain bound joint_const.joint_name = "right_j6" joint_const.position = 1.7314052734375 # the bound to be achieved is [position - tolerance_below, position + tolerance_above] joint_const.tolerance_above = .2 joint_const.tolerance_below = .2 # A weighting factor for this constraint (denotes relative importance to other constraints. Closer to zero means less important) joint_const.weight = .5 #for stage 2 # orien_const1 = OrientationConstraint() # orien_const1.link_name = "right_gripper"; # orien_const1.header.frame_id = "ar_marker_1"; # #orien_const.orientation.y = np.pi/2 # #orien_const.orientation.w = 1; # orien_const1.absolute_x_axis_tolerance = 0.5; # orien_const1.absolute_y_axis_tolerance = 0.5; # orien_const1.absolute_z_axis_tolerance = 0.5; # orien_const1.weight = 1.0; #rospy.sleep(1.0) # while not rospy.is_shutdown(): # while not rospy.is_shutdown(): try: #right_gripper_tip goal_1 = PoseStamped() goal_1.header.frame_id = "ar_marker_1" #x, y, and z position goal_1.pose.position.x = -0.28 goal_1.pose.position.y = -0.05 #removing the 6th layer goal_1.pose.position.z = .38 #0.0825 # z was .1 #Orientation as a quaternion goal_1.pose.orientation.x = 0 goal_1.pose.orientation.y = 0 goal_1.pose.orientation.z = 0 goal_1.pose.orientation.w = 1 plan = planner.plan_to_pose_joint(goal_1, [joint_const]) if not planner.execute_plan(plan): raise Exception("Execution failed0") except Exception as e: print e
def inverse_kinematics(): # Construct the request request = GetPositionIKRequest() request.ik_request.group_name = "right_arm" request.ik_request.ik_link_name = "right_gripper" request.ik_request.attempts = 50 request.ik_request.pose_stamped.header.frame_id = "base" # Create joint constraints #This is joint constraint will need to be set at the group constraints = Constraints() joint_constr = JointConstraint() joint_constr.joint_name = "right_j0" joint_constr.position = TORSO_POSITION joint_constr.tolerance_above = TOLERANCE joint_constr.tolerance_below = TOLERANCE joint_constr.weight = 0.5 constraints.joint_constraints.append(joint_constr) # Get the transformed AR Tag (x,y,z) coordinates # Only care about the x coordinate of AR tag; tells use # how far away wall is # x,y, z tell us the origin of the AR Tag x_coord = board_x # DONT CHANGE y_coord = bounding_points[0] z_coord = bounding_points[1] y_width = bounding_points[2] z_height = bounding_points[3] #Creating Path Planning waypoints = [] z_bais = 0 print( "OMMMMMMMMMMMMMMMMMMMMMMMMMGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGG!!!!!!!!!!!!!!" ) print(bounding_points) for i in range(int(float(z_height) / .03)): target_pose = Pose() target_pose.position.x = float(x_coord - PLANNING_BIAS) if i % 2 == 0: #Starting positions target_pose.position.y = float(y_coord) else: target_pose.position.y = y_coord + float(y_width) #Starting positions target_pose.position.z = float(z_coord + z_bais) target_pose.orientation.y = 1.0 / 2**(1 / 2.0) target_pose.orientation.w = 1.0 / 2**(1 / 2.0) waypoints.append(target_pose) z_bais += .03 #Set the desired orientation for the end effector HERE request.ik_request.pose_stamped.pose = target_pose try: #Send the request to the service response = compute_ik(request) group = MoveGroupCommander("right_arm") group.set_max_velocity_scaling_factor(0.75) #Set the desired orientation for the end effector HERE request.ik_request.pose_stamped.pose = target_pose #Creating a Robot Trajectory for the Path Planning jump_thres = 0.0 eef_step = 0.1 path, fraction = group.compute_cartesian_path([target_pose], eef_step, jump_thres) print("Path fraction: {}".format(fraction)) #Setting position and orientation target group.set_pose_target(request.ik_request.pose_stamped) #Setting the Joint constraint group.set_path_constraints(constraints) if fraction < 0.5: group.go() else: group.execute(path) if i < int(float(z_height) / 0.03) and i > 0: target2 = target_pose target2.position.z += 0.03 path, fraction = group.compute_cartesian_path([target2], eef_step, jump_thres) group.set_path_constraints(constraints) group.execute(path) except rospy.ServiceException, e: print "Service call failed: %s" % e
workspace_parameters.header.stamp = rospy.Time.now() workspace_parameters.header.frame_id = "/BASE" 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 = "/BASE" start_state.joint_state.name = ["j1", "j2", "j3", "j4", "j5", "flange"] start_state.joint_state.position = [-0.2569046038066598, -0.8442722962923348, 1.849082034218144, 0.26825374068443164, -0.04090683809444329, 5.745512865657193] start_state.joint_state.velocity = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] jc2 = JointConstraint() jc2.joint_name = "j1" jc2.position = -1.37353344093 jc2.tolerance_above = 0.0001 jc2.tolerance_below = 0.0001 jc2.weight = 1.0 jc3 = JointConstraint() jc3.joint_name = "j2" jc3.position = -1.45013378543 jc3.tolerance_above = 0.0001 jc3.tolerance_below = 0.0001 jc3.weight = 1.0 jc4 = JointConstraint() jc4.joint_name = "j3" jc4.position = 2.18173030842 jc4.tolerance_above = 0.0001 jc4.tolerance_below = 0.0001
def place(self, object_name, position, distance = 0.1): if not self.is_inside_workspace(position.x, position.y, position.z): rospy.loginfo('***** GOAL POSE IS OUT OF ROBOT WORKSPACE *****') rospy.loginfo('Stop placing') return pose = Pose() pose.position = position q = quaternion_from_euler(0.0, numpy.deg2rad(90.0), numpy.deg2rad(180.0)) pose.orientation = Quaternion(*q) pose.position.z += distance # Add contraint constraint = Constraints() joints = self.get_joints_value() joint_constraint = JointConstraint() joint_constraint.joint_name = "wrist_2_joint" joint_constraint.position = joints[4] joint_constraint.tolerance_above = 0.8 joint_constraint.tolerance_below = 0.8 joint_constraint.weight = 0.2 constraint.joint_constraints.append(joint_constraint) joint_constraint = JointConstraint() joint_constraint.joint_name = "wrist_3_joint" joint_constraint.position = joints[5] joint_constraint.tolerance_above = 0.8 joint_constraint.tolerance_below = 0.8 joint_constraint.weight = 0.2 constraint.joint_constraints.append(joint_constraint) joint_constraint = JointConstraint() joint_constraint.joint_name = "wrist_1_joint" joint_constraint.position = joints[3] joint_constraint.tolerance_above = 1.0 joint_constraint.tolerance_below = 1.0 joint_constraint.weight = 0.1 constraint.joint_constraints.append(joint_constraint) joint_constraint = JointConstraint() joint_constraint.joint_name = "elbow_joint" joint_constraint.position = joints[2] joint_constraint.tolerance_above = 1.2 joint_constraint.tolerance_below = 1.2 joint_constraint.weight = 0.1 constraint.joint_constraints.append(joint_constraint) orientation_constraint = OrientationConstraint() now = rospy.Time.now() orientation_constraint.header.stamp = now orientation_constraint.header.frame_id = "base_link" orientation_constraint.link_name = "ee_link" orientation_constraint.orientation = pose.orientation orientation_constraint.absolute_x_axis_tolerance = 10 orientation_constraint.absolute_y_axis_tolerance = 1.0 orientation_constraint.absolute_z_axis_tolerance = 10 orientation_constraint.weight = 0.4 constraint.orientation_constraints.append(orientation_constraint) self.arm.set_path_constraints(constraint) # self.arm.set_goal_position_tolerance(0.015) rospy.loginfo('Start placing') # self.send_message('Start placing') self.arm.set_max_velocity_scaling_factor(0.2) self.arm.set_workspace([-0.8, -0.8, -0.2, 0.8, 0.8, 0.3]) self.move_pose_arm(pose) rospy.sleep(1) self.arm.clear_path_constraints() # self.arm.set_goal_tolerance(0.01) # move down waypoints = [] wpose = self.arm.get_current_pose().pose wpose.position.z -= distance waypoints.append(copy.deepcopy(wpose)) (plan, fraction) = self.arm.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) # jump_threshold self.arm.execute(plan, wait=True) # place self.gripper_release() self.arm.detach_object(object_name) self.clean_scene(object_name) rospy.sleep(0.5) # move up waypoints = [] wpose = self.arm.get_current_pose().pose wpose.position.z += distance waypoints.append(copy.deepcopy(wpose)) (plan, fraction) = self.arm.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) # jump_threshold self.arm.execute(plan, wait=True) rospy.loginfo('Place finished') # self.send_message('Place finished') self.arm.set_workspace([-1, -1, -0.2, 1, 1, 1.0])
def kinect_planner(): global fresh_data global joint_target, pose_target, goal global joints_req, pose_req moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('kinect_trajectory_planner', anonymous=True) rate = rospy.Rate(0.5) #rospy.sleep(3) # Instantiate a RobotCommander object. robot = moveit_commander.RobotCommander() # Instantiate a PlanningSceneInterface object (interface to the world surrounding the robot). scene = moveit_commander.PlanningSceneInterface() print "Remember to disable firewall if it's not working" # Instantiate MoveGroupCommander objects for arms and Kinect2. group = moveit_commander.MoveGroupCommander("Kinect2_Target") group_left_arm = moveit_commander.MoveGroupCommander("left_arm") group_right_arm = moveit_commander.MoveGroupCommander("right_arm") # We create this DisplayTrajectory publisher to publish # trajectories for RVIZ to visualize. display_trajectory_publisher = rospy.Publisher('planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=5) # Set the planner for Moveit group.set_planner_id("RRTConnectkConfigDefault") group.set_planning_time(8) group.set_pose_reference_frame('base_footprint') # Setting tolerance group.set_goal_tolerance(0.08) group.set_num_planning_attempts(10) # Suscribing to the desired pose topic target = rospy.Subscriber("desired_pose", PoseStamped, callback) target_joint = rospy.Subscriber("desired_joints", pose_w_joints, callback_joints) # Locating the arms and the Kinect2 sensor group_left_arm_values = group_left_arm.get_current_joint_values() group_right_arm_values = group_right_arm.get_current_joint_values() #group_kinect_values = group_kinect.get_current_joint_values() neck_init_joints = group.get_current_joint_values() neck_init_joints[0] = -1.346 neck_init_joints[1] = -1.116 neck_init_joints[2] = -2.121 neck_init_joints[3] = 0.830 neck_init_joints[4] = 1.490 neck_init_joints[5] = 0.050 neck_init_joints[6] = 0 neck_init_joints[7] = 0 neck_init_joints[8] = 0 neck_init_joints[9] = 0 # Creating a box to limit the arm position box_pose = PoseStamped() box_pose.pose.orientation.w = 1 box_pose.pose.position.x = 0.6 box_pose.pose.position.y = 0.03 box_pose.pose.position.z = 1.5 box_pose.header.frame_id = 'base_footprint' scene.add_box('box1', box_pose, (0.4, 0.4, 0.1)) rospy.sleep(2) # Defining position constraints for the trajectory of the kinect neck_const = Constraints() neck_const.name = 'neck_constraints' target_const = JointConstraint() target_const.joint_name = "neck_joint_end" target_const.position = 0.95 target_const.tolerance_above = 0.45 target_const.tolerance_below = 0.05 target_const.weight = 0.9 # Importance of this constraint neck_const.joint_constraints.append(target_const) # Talking to the robot #client = actionlib.SimpleActionClient('/Kinect2_Target_controller/follow_joint_trajectory', FollowJointTrajectoryAction) client = actionlib.SimpleActionClient('/neck/follow_joint_trajectory', FollowJointTrajectoryAction) print "====== Waiting for server..." client.wait_for_server() print "====== Connected to server" while not rospy.is_shutdown(): print('.') skip_iter = False if fresh_data == True: #If a new pose is received, plan. # Update arms position group_left_arm_values = group_left_arm.get_current_joint_values() group_right_arm_values = group_right_arm.get_current_joint_values() # Set the target pose (or joint values) and generate a plan if pose_req: group.set_pose_target(pose_target) if joints_req: print('about to set joint target') print(joint_target) try: if len(joint_target) == 6: #Setting also values for the four virtual joints (one prismatic, and three rotational) new_joint_target = joint_target + [0.1]*4 else: new_joint_target = joint_target print('setting: '), print new_joint_target group.set_joint_value_target(new_joint_target) except: print('cannot set joint target') skip_iter = True print "=========== Calculating trajectory... \n" # Generate several plans and compare them to get the shorter one plan_opt = dict() differ = dict() if not skip_iter: try: num = 1 rep = 0 # Generate several plans and compare them to get the shortest one #for num in range(1,8): while num < 7: num += 1 plan_temp = group.plan() move(plan_temp) plan_opt[num] = goal.trajectory diff=0 for point in goal.trajectory.points: # calculate the distance between initial pose and planned one for i in range(0,6): diff = abs(neck_init_joints[i] - point.positions[i])+abs(diff) differ[num] = diff # If the current plan is good, take it if diff < 110: break # If plan is too bad, don't consider it if diff > 400: num = num - 1 print "Plan is too long. Replanning." rep = rep + 1 if rep > 4: num = num +1 # If no plan was found... if differ == {}: print "---- Fail: No motion plan found. No execution attempted. Probably robot joints are out of range." break else: # Select the shortest plan min_plan = min(differ.itervalues()) select = [k for k, value in differ.iteritems() if value == min_plan] goal.trajectory = plan_opt[select[0]] #print " Plan difference:======= ", differ #print " Selected plan:========= ", select[0] # Remove the last 4 names and data from each point (dummy joints) before sending the goal goal.trajectory.joint_names = goal.trajectory.joint_names[:6] for point in goal.trajectory.points: point.positions = point.positions[:6] point.velocities = point.velocities[:6] point.accelerations = point.accelerations[:6] print "Sending goal" client.send_goal(goal) print "Waiting for result" client.wait_for_result() # Change the position of the virtual joint to avoid collision neck_joints = group.get_current_joint_values() print('neck joints:') print neck_joints #neck_joints[6] = 0.7 #group.set_joint_value_target(neck_joints) #group.go(wait=True) except (KeyboardInterrupt, SystemExit): client.cancel_goal() raise rate.sleep() fresh_data = False rate.sleep()
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
result = fk.getFK('arm_right_7_link', current_joint_states.name, correct_js.position, 'base_link') rospy.loginfo("Result of current robot pose FK is: " + str(result)) rs = RobotState() rs.joint_state = correct_js drs = DisplayRobotState() drs.state = rs rs_pub.publish(drs) rospy.loginfo("Published current robot state") rospy.sleep(2.0) c = Constraints() jc = JointConstraint() jc.joint_name = 'arm_right_1_link' jc.position = 0.0 jc.tolerance_above = 0.00001 jc.tolerance_below = 0.00001 jc.weight = 1.0 c.joint_constraints.append(jc) rospy.loginfo("Result without constraints:") resultik = ik.getIK("right_arm", "right_arm_7_link", result.pose_stamped[0], False, 0, rs) rospy.loginfo(str(resultik)) rs = RobotState() rs.joint_state = resultik.solution.joint_state drs = DisplayRobotState() drs.state = rs rs_pub.publish(drs) rospy.loginfo("Published solution with False collision avoidance robot state") rospy.sleep(5.0)
def main(): # Initialize the node rospy.init_node("moveit_client") # Create the SimpleActionClient, passing the type of the action # (MoveGroupAction) to the constructor. client = actionlib.SimpleActionClient("move_group", MoveGroupAction) # Wait until the action server has started up and started # listening for goals. client.wait_for_server() # Creates a goal to send to the action server. goal = MoveGroupGoal() # ----------------Construct the goal message (start)---------------- joint_names = [ "head_pan", "left_s0", "left_s1", "left_e0", "left_e1", "left_w0", "left_w1", "left_w2", "right_s0", "right_s1", "right_e0", "right_e1", "right_w0", "right_w1", "right_w2", ] # Set parameters for the planner goal.request.group_name = "both_arms" goal.request.num_planning_attempts = 1 goal.request.allowed_planning_time = 5.0 # Define the workspace in which the planner will search for solutions goal.request.workspace_parameters.min_corner.x = -1 goal.request.workspace_parameters.min_corner.y = -1 goal.request.workspace_parameters.min_corner.z = -1 goal.request.workspace_parameters.max_corner.x = 1 goal.request.workspace_parameters.max_corner.y = 1 goal.request.workspace_parameters.max_corner.z = 1 goal.request.start_state.joint_state.header.frame_id = "base" # Set the start state for the trajectory goal.request.start_state.joint_state.name = joint_names goal.request.start_state.joint_state.position = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, ] goal.request.start_state.joint_state.velocity = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, ] # Tell MoveIt whether to execute the trajectory after planning it goal.planning_options.plan_only = True # Set the goal position of the robot # Note that the goal is specified with a collection of individual # joint constraints, rather than a vector of joint angles arm_joint_names = joint_names[1:] print("Joint angles...? Are coming one at a time. Give them to me in radians") target_joint_angles = [] for joint in joint_names: str = raw_input(joint + "\n-->") val = float(str) target_joint_angles.append(val) print("I think that you want me to go to...") print(target_joint_angles) raw_input("Press any key to continue") # target_joint_angles = [0.5, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] tolerance = 0.0001 consts = [] for i in range(len(arm_joint_names)): const = JointConstraint() const.joint_name = arm_joint_names[i] const.position = target_joint_angles[i] const.tolerance_above = tolerance const.tolerance_below = tolerance const.weight = 1.0 consts.append(const) goal.request.goal_constraints.append(Constraints(name="", joint_constraints=consts)) # ---------------Construct the goal message (end)----------------- # Send the goal to the action server. client.send_goal(goal) # Wait for the server to finish performing the action. client.wait_for_result() # Print out the result of executing the action print(client.get_result())
def main(): #Initialize the node rospy.init_node('moveit_client') # Create the SimpleActionClient, passing the type of the action # (MoveGroupAction) to the constructor. client = actionlib.SimpleActionClient('move_group', MoveGroupAction) # Wait until the action server has started up and started # listening for goals. client.wait_for_server() # Creates a goal to send to the action server. goal = MoveGroupGoal() #----------------Construct the goal message (start)---------------- joint_names = [ 'head_pan', 'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1', 'left_w2', 'right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2' ] #Set parameters for the planner goal.request.group_name = 'both_arms' goal.request.num_planning_attempts = 1 goal.request.allowed_planning_time = 5.0 #Define the workspace in which the planner will search for solutions goal.request.workspace_parameters.min_corner.x = -1 goal.request.workspace_parameters.min_corner.y = -1 goal.request.workspace_parameters.min_corner.z = -1 goal.request.workspace_parameters.max_corner.x = 1 goal.request.workspace_parameters.max_corner.y = 1 goal.request.workspace_parameters.max_corner.z = 1 goal.request.start_state.joint_state.header.frame_id = "base" #Set the start state for the trajectory goal.request.start_state.joint_state.name = joint_names goal.request.start_state.joint_state.position = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] goal.request.start_state.joint_state.velocity = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] #Tell MoveIt whether to execute the trajectory after planning it goal.planning_options.plan_only = True #Set the goal position of the robot #Note that the goal is specified with a collection of individual #joint constraints, rather than a vector of joint angles bagel = raw_input('Please enter in 15 joint angles and press enter: ') bagel = bagel.split() i = 0 for x in bagel: bagel[i] = float(x) i += 1 arm_joint_names = joint_names[1:] target_joint_angles = bagel tolerance = 0.0001 consts = [] for i in range(len(arm_joint_names)): const = JointConstraint() const.joint_name = arm_joint_names[i] const.position = target_joint_angles[i] const.tolerance_above = tolerance const.tolerance_below = tolerance const.weight = 1.0 consts.append(const) goal.request.goal_constraints.append( Constraints(name='', joint_constraints=consts)) #---------------Construct the goal message (end)----------------- # Send the goal to the action server. client.send_goal(goal) # Wait for the server to finish performing the action. client.wait_for_result() # Print out the result of executing the action print(client.get_result())
def main(): #Initialize the node rospy.init_node('moveit_client') # Create the SimpleActionClient, passing the type of the action # (MoveGroupAction) to the constructor. client = actionlib.SimpleActionClient('move_group', MoveGroupAction) # Wait until the action server has started up and started # listening for goals. client.wait_for_server() # Creates a goal to send to the action server. goal = MoveGroupGoal() #----------------Construct the goal message (start)---------------- joint_names = ['head_pan', 'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1', 'left_w2', 'right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2'] #Set parameters for the planner goal.request.group_name = 'both_arms' goal.request.num_planning_attempts = 1 goal.request.allowed_planning_time = 5.0 #Define the workspace in which the planner will search for solutions goal.request.workspace_parameters.min_corner.x = -1 goal.request.workspace_parameters.min_corner.y = -1 goal.request.workspace_parameters.min_corner.z = -1 goal.request.workspace_parameters.max_corner.x = 1 goal.request.workspace_parameters.max_corner.y = 1 goal.request.workspace_parameters.max_corner.z = 1 goal.request.start_state.joint_state.header.frame_id = "base" #Set the start state for the trajectory goal.request.start_state.joint_state.name = joint_names goal.request.start_state.joint_state.position = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] goal.request.start_state.joint_state.velocity = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] print 'Enter start state:' try: for i in range(len(joint_names)): val = float(raw_input(joint_names[i]+':')) goal.request.start_state.joint_state.position[i] = val except: print 'Using defaults for remaining joints' print goal.request.start_state.joint_state.position #Tell MoveIt whether to execute the trajectory after planning it goal.planning_options.plan_only = True #Set the goal position of the robot #Note that the goal is specified with a collection of individual #joint constraints, rather than a vector of joint angles arm_joint_names = joint_names[1:] target_joint_angles = [0.5, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] print 'Enter goal state:' try: for i in range(len(target_joint_angles)): val = float(raw_input(joint_names[i]+':')) target_joint_angles[i] = val except: print 'Using defaults for remaining joints' print target_joint_angles tolerance = 0.0001 consts = [] for i in range(len(arm_joint_names)): const = JointConstraint() const.joint_name = arm_joint_names[i] const.position = target_joint_angles[i] const.tolerance_above = tolerance const.tolerance_below = tolerance const.weight = 1.0 consts.append(const) goal.request.goal_constraints.append(Constraints(name='', joint_constraints=consts)) #---------------Construct the goal message (end)----------------- # Send the goal to the action server. client.send_goal(goal) # Wait for the server to finish performing the action. client.wait_for_result() # Print out the result of executing the action print(client.get_result())