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 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 _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 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