def _cmd_to_request(self, robot): """Transforms the gripper command to a MotionPlanRequest.""" req = MotionPlanRequest() # Set general info req.planner_id = "PTP" req.group_name = self._planning_group req.max_velocity_scaling_factor = self._vel_scale * robot.global_motion_factor req.max_acceleration_scaling_factor = self._acc_scale * robot.global_motion_factor req.allowed_planning_time = 1.0 # Set an empty diff as start_state => the current state is used by the planner req.start_state.is_diff = True # create goal constraints goal_constraints = Constraints() if isinstance(self._goal, (float, int, long)): joint_names = robot._robot_commander.get_group(self._planning_group).get_active_joints() if len(joint_names) != 1: raise IndexError("PG70 should have only one joint. But group " + req.group_name + " contains " + str(len(joint_names)) + " joints.") joint_constraint = JointConstraint() joint_constraint.joint_name = joint_names[0] joint_constraint.position = float(self._goal) joint_constraint.weight = 1 goal_constraints.joint_constraints.append(joint_constraint) else: raise NotImplementedError("Unknown type of goal is given.") req.goal_constraints.append(goal_constraints) return req
def create_basic_mp_position_request( planning_frame, link_name, target_point_offset, 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 position_constraints = [] position_constraint = PositionConstraint() header = std_msgs.msg.Header() header.frame_id = planning_frame position_constraint.header = header position_constraint.link_name = link_name position_constraint.target_point_offset = target_point_offset position_constraints = [position_constraint] constraint = Constraints() constraint.position_constraints = position_constraints constraints = [constraint] motion_plan_request.goal_constraints = constraints return motion_plan_request
def _cmd_to_request(self, robot): """Transforms the given command to a MotionPlanRequest.""" req = MotionPlanRequest() # Set general info req.planner_id = self._planner_id req.group_name = self._planning_group req.max_velocity_scaling_factor = self._vel_scale req.max_acceleration_scaling_factor = self._acc_scale req.allowed_planning_time = 1.0 # Set an empty diff as start_state => the current state is used by the planner req.start_state.is_diff = True # Set goal constraint if self._goal is None: raise NameError("Goal is not given.") goal_constraints = Constraints() # goal as Pose in Cartesian space if isinstance(self._goal, Pose): goal_pose = self._get_goal_pose(robot) robot_reference_frame = robot._robot_commander.get_planning_frame() goal_constraints.orientation_constraints.append( _to_ori_constraint(goal_pose, robot_reference_frame, self._target_link)) goal_constraints.position_constraints.append( _to_pose_constraint(goal_pose, robot_reference_frame, self._target_link)) # goal as list of int or float in joint space elif isinstance(self._goal, list): joint_names = robot._robot_commander.get_group( self._planning_group).get_active_joints() joint_values = self._get_joint_pose(robot) if len(joint_names) != len(joint_values): raise IndexError( "Given joint goal does not match the planning group " + req.group_name + ".") for joint_name, joint_value in zip(joint_names, joint_values): joint_constraint = JointConstraint() joint_constraint.joint_name = joint_name joint_constraint.position = joint_value joint_constraint.weight = 1 goal_constraints.joint_constraints.append(joint_constraint) else: raise NotImplementedError("Unknown type of goal is given.") req.goal_constraints.append(goal_constraints) return req
def constraint_planner(start_robot_state, goal_config, group_handle, planner_name, planning_attemps, planning_time): planning_workspace = WorkspaceParameters(); planning_workspace.header.frame_id = "/base_link"; planning_workspace.header.stamp = rospy.Time.now(); planning_workspace.min_corner.x = -1; planning_workspace.min_corner.y = -1; planning_workspace.min_corner.z = -1; planning_workspace.max_corner.x = 1; planning_workspace.max_corner.y = 1; planning_workspace.max_corner.z = 1; # Set Start start_state = RobotState(); start_state = start_robot_state; print ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>"; #print start_robot_state; # Set Goal goal_state = Constraints(); Jnt_constraint = setJointConstraints(goal_config, group_handle); goal_state.joint_constraints = Jnt_constraint; #print Jnt_constraint; # Set Constraints des_w = -0.070099; des_x = 0.41382; des_y = -0.57302; des_z = 0.70391; rotation_constraints = setOrientationConstraints(des_w,des_x,des_y,des_z,group_handle, weight = 1.0); # Generating Request planningRequest = MotionPlanRequest(); planningRequest.workspace_parameters = planning_workspace; planningRequest.start_state = start_robot_state; planningRequest.goal_constraints.append(goal_state); # Setting Constraint planningRequest.path_constraints.name = 'scoop_constraint'; planningRequest.path_constraints.orientation_constraints.append(rotation_constraints); #traj_constraint = Constraints(); #traj_constraint.orientation_constraints.append(rotation_constraints); #planningRequest.trajectory_constraints.constraints.append(traj_constraint); planningRequest.planner_id = "RRTConnectkConfigDefault"; planningRequest.group_name = group_handle.get_name(); planningRequest.num_planning_attempts = planning_attemps; planningRequest.allowed_planning_time = planning_time; planningRequest.max_velocity_scaling_factor = 1.0; Planning_req.publish(planningRequest);
def set_global_defaults( self, group ): """ Initializes the proxy with default parameters to use with connections @param group : string move group name """ # Default planning parameters for new connections num_planning_attempts=5 allowed_planning_time=10.0 plan_only=True planner_id='RRTConnectkConfigDefault' # Set the default plan request data plan_request = MotionPlanRequest() plan_request.start_state.is_diff = True # Flags start state as empty to use current state on server plan_request.num_planning_attempts = num_planning_attempts plan_request.allowed_planning_time = allowed_planning_time plan_request.planner_id = planner_id plan_request.group_name = group #@TODO - retrieve from ROS parameters ProxyMoveItClient._motion_plan_requests[group] = plan_request ProxyMoveItClient._default_motion_plan_requests[group] = copy.deepcopy(plan_request) planning_options = PlanningOptions() planning_options.plan_only = plan_only planning_options.planning_scene_diff.robot_state.is_diff = True # Flags start state as empty to use current state on server #@TODO - retrieve from ROS parameters ProxyMoveItClient._planning_options[group] = planning_options ProxyMoveItClient._default_planning_options[group] = copy.deepcopy(planning_options) # Set up the default joint constraints joint_constraints = {} for name in ProxyMoveItClient._joint_names[group]: joint_constraints[name] = JointConstraint(joint_name=name, tolerance_above=0.05, tolerance_below=0.05,weight=1.0) #@TODO - retrieve from ROS parameters ProxyMoveItClient._joint_constraints[group] = joint_constraints ProxyMoveItClient._default_joint_constraints[group] = copy.deepcopy(joint_constraints) # @TODO - add real constraints ProxyMoveItClient._position_constraints[group] = [] ProxyMoveItClient._default_position_constraints[group] = [] ProxyMoveItClient._orientation_constraints[group] = [] ProxyMoveItClient._default_orientation_constraints[group] = [] ProxyMoveItClient._visibility_constraints[group] = [] ProxyMoveItClient._default_visibility_constraints[group] = [] ProxyMoveItClient._robot_states[group] = RobotState() ProxyMoveItClient._default_robot_states[group] = RobotState()
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 motionPlanToPose(self, pose, tolerance=0.01, wait=True, **kwargs): ''' Move the arm to set of joint position goals :param joints: joint names for which the target position is specified. :param positions: target joint positions :param tolerance: allowed tolerance in the final joint positions. :param wait: if enabled, makes the fuctions wait until the target joint position is reached :type joints: list of string element type :type positions: list of float element type :type tolerance: float :type wait: bool ''' # Check arguments supported_args = ("max_velocity_scaling_factor", "max_acceleration_scaling_factor", "planner_id", "planning_scene_diff", "planning_time", "plan_only", "start_state") for arg in kwargs.keys(): if not arg in supported_args: rospy.loginfo("motionPlanToPose: unsupported argument: %s", arg) # Create goal g = MotionPlanRequest() # 1. fill in request workspace_parameters # 2. fill in request start_state try: g.start_state = kwargs["start_state"] except KeyError: g.start_state.is_diff = True # 3. fill in request goal_constraints c1 = Constraints() c1.position_constraints.append(PositionConstraint()) c1.position_constraints[0].header.frame_id = self._fixed_frame c1.position_constraints[0].link_name = self._gripper_frame # c1.position_constraints[0].target_point_offset b = BoundingVolume() s = SolidPrimitive() s.dimensions = [tolerance] s.type = s.SPHERE b.primitives.append(s) b.primitive_poses.append(pose) c1.position_constraints[0].constraint_region = b c1.position_constraints[0].weight = 1.0 c1.orientation_constraints.append(OrientationConstraint()) c1.orientation_constraints[0].header.frame_id = self._fixed_frame c1.orientation_constraints[0].orientation = pose.orientation c1.orientation_constraints[0].link_name = self._gripper_frame c1.orientation_constraints[0].absolute_x_axis_tolerance = tolerance c1.orientation_constraints[0].absolute_y_axis_tolerance = tolerance c1.orientation_constraints[0].absolute_z_axis_tolerance = tolerance c1.orientation_constraints[0].weight = 1.0 g.goal_constraints.append(c1) # 4. fill in request path constraints # 5. fill in request trajectory constraints # 6. fill in request planner id try: g.planner_id = kwargs["planner_id"] except KeyError: if self.planner_id: g.planner_id = self.planner_id # 7. fill in request group name g.group_name = self._group # 8. fill in request number of planning attempts try: g.num_planning_attempts = kwargs["num_attempts"] except KeyError: g.num_planning_attempts = 1 # 9. fill in request allowed planning time try: g.allowed_planning_time = kwargs["planning_time"] except KeyError: g.allowed_planning_time = self.planning_time # 10. Fill in velocity scaling factor try: g.max_velocity_scaling_factor = kwargs["max_velocity_scaling_factor"] except KeyError: pass # do not fill in at all # 11. Fill in acceleration scaling factor try: g.max_velocity_scaling_factor = kwargs["max_acceleration_scaling_factor"] except KeyError: pass # do not fill in at all result = self._mp_service(g) traj = result.motion_plan_response.trajectory.joint_trajectory.points if len(traj) < 1: rospy.logwarn('No motion plan found.') return None return traj
def motionPlanToJointPosition(self, joints, positions, tolerance=0.01, wait=True, **kwargs): ''' Move the arm to set of joint position goals :param joints: joint names for which the target position is specified. :param positions: target joint positions :param tolerance: allowed tolerance in the final joint positions. :param wait: if enabled, makes the fuctions wait until the target joint position is reached :type joints: list of string element type :type positions: list of float element type :type tolerance: float :type wait: bool ''' # Check arguments supported_args = ("max_velocity_scaling_factor", "max_acceleration_scaling_factor", "planner_id", "planning_scene_diff", "planning_time", "plan_only", "start_state") for arg in kwargs.keys(): if not arg in supported_args: rospy.loginfo("motionPlanToPose: unsupported argument: %s", arg) # Create goal g = MotionPlanRequest() # 1. fill in request workspace_parameters # 2. fill in request start_state try: g.start_state = kwargs["start_state"] except KeyError: g.start_state.is_diff = True # 3. fill in request goal_constraints c1 = Constraints() for i in range(len(joints)): c1.joint_constraints.append(JointConstraint()) c1.joint_constraints[i].joint_name = joints[i] c1.joint_constraints[i].position = positions[i] c1.joint_constraints[i].tolerance_above = tolerance c1.joint_constraints[i].tolerance_below = tolerance c1.joint_constraints[i].weight = 1.0 g.goal_constraints.append(c1) # 4. fill in request path constraints # 5. fill in request trajectory constraints # 6. fill in request planner id try: g.planner_id = kwargs["planner_id"] except KeyError: if self.planner_id: g.planner_id = self.planner_id # 7. fill in request group name g.group_name = self._group # 8. fill in request number of planning attempts try: g.num_planning_attempts = kwargs["num_attempts"] except KeyError: g.num_planning_attempts = 1 # 9. fill in request allowed planning time try: g.allowed_planning_time = kwargs["planning_time"] except KeyError: g.allowed_planning_time = self.planning_time # 10. Fill in velocity scaling factor try: g.max_velocity_scaling_factor = kwargs["max_velocity_scaling_factor"] except KeyError: pass # do not fill in at all # 11. Fill in acceleration scaling factor try: g.max_velocity_scaling_factor = kwargs["max_acceleration_scaling_factor"] except KeyError: pass # do not fill in at all result = self._mp_service(g) traj = result.motion_plan_response.trajectory.joint_trajectory.points if len(traj) < 1: rospy.logwarn('No motion plan found.') return None return traj