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 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 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
jc0.joint_name = "flange" jc0.position = -4.43859335239 jc0.tolerance_above = 0.0001 jc0.tolerance_below = 0.0001 jc0.weight = 1.0 cons = Constraints() cons.name = "" cons.joint_constraints.append(jc2) cons.joint_constraints.append(jc3) cons.joint_constraints.append(jc4) cons.joint_constraints.append(jc5) cons.joint_constraints.append(jc1) cons.joint_constraints.append(jc0) msg.workspace_parameters = workspace_parameters msg.start_state = start_state msg.goal_constraints.append(cons) msg.num_planning_attempts = 1 msg.allowed_planning_time = 5.0 msg.group_name = "manipulator" move_group_goal.request = msg client.send_goal(move_group_goal) client.wait_for_result(rospy.Duration.from_sec(5.0)) except rospy.ROSInterruptException, e:
def joint_position_callback(joints): global plan_only fixed_frame = rospy.get_param("/fixed_frame") client = actionlib.SimpleActionClient('move_group', MoveGroupAction) client.wait_for_server() move_group_goal = MoveGroupGoal() try: msg = MotionPlanRequest() workspace_parameters = WorkspaceParameters() workspace_parameters.header.stamp = rospy.Time.now() workspace_parameters.header.frame_id = fixed_frame workspace_parameters.min_corner = Vector3(-1.0, -1.0, -1.0) workspace_parameters.max_corner = Vector3(1.0, 1.0, 1.0) start_state = RobotState() # start_state.joint_state.header.stamp = rospy.Time.now() start_state.joint_state.header.frame_id = fixed_frame start_state.joint_state.name = [] start_state.joint_state.position = [] cons = Constraints() cons.name = "" i = 0 for dim in joints.start_joint.layout.dim: start_state.joint_state.name.append(dim.label) start_state.joint_state.position.append(joints.start_joint.data[i]) jc = JointConstraint() jc.joint_name = dim.label jc.position = joints.goal_joint.data[i] jc.tolerance_above = 0.0001 jc.tolerance_below = 0.0001 jc.weight = 1.0 i = i + 1 cons.joint_constraints.append(jc) msg.workspace_parameters = workspace_parameters msg.start_state = start_state msg.goal_constraints.append(cons) msg.num_planning_attempts = 1 msg.allowed_planning_time = 5.0 msg.group_name = joints.group_name move_group_goal.request = msg if joints.plan_only: plan_only = True move_group_goal.planning_options.plan_only = True else: plan_only = False client.send_goal(move_group_goal) client.wait_for_result(rospy.Duration.from_sec(5.0)) except rospy.ROSInterruptException, e: print "failed: %s"%e
jc0.joint_name = "flange" jc0.position = -4.43859335239 jc0.tolerance_above = 0.0001 jc0.tolerance_below = 0.0001 jc0.weight = 1.0 cons = Constraints() cons.name = "" cons.joint_constraints.append(jc2) cons.joint_constraints.append(jc3) cons.joint_constraints.append(jc4) cons.joint_constraints.append(jc5) cons.joint_constraints.append(jc1) cons.joint_constraints.append(jc0) msg.workspace_parameters = workspace_parameters msg.start_state = start_state msg.goal_constraints.append(cons) msg.num_planning_attempts = 1 msg.allowed_planning_time = 5.0 msg.group_name = "manipulator" move_group_goal.request = msg client.send_goal(move_group_goal) client.wait_for_result(rospy.Duration.from_sec(5.0)) except rospy.ROSInterruptException, e: print "failed: %s" % e
def joint_position_callback(joints): global plan_only fixed_frame = rospy.get_param("/fixed_frame") client = actionlib.SimpleActionClient('move_group', MoveGroupAction) client.wait_for_server() move_group_goal = MoveGroupGoal() try: msg = MotionPlanRequest() workspace_parameters = WorkspaceParameters() workspace_parameters.header.stamp = rospy.Time.now() workspace_parameters.header.frame_id = fixed_frame workspace_parameters.min_corner = Vector3(-1.0, -1.0, -1.0) workspace_parameters.max_corner = Vector3(1.0, 1.0, 1.0) start_state = RobotState() # start_state.joint_state.header.stamp = rospy.Time.now() start_state.joint_state.header.frame_id = fixed_frame start_state.joint_state.name = [] start_state.joint_state.position = [] cons = Constraints() cons.name = "" i = 0 for dim in joints.start_joint.layout.dim: start_state.joint_state.name.append(dim.label) start_state.joint_state.position.append(joints.start_joint.data[i]) jc = JointConstraint() jc.joint_name = dim.label jc.position = joints.goal_joint.data[i] jc.tolerance_above = 0.0001 jc.tolerance_below = 0.0001 jc.weight = 1.0 i = i + 1 cons.joint_constraints.append(jc) msg.workspace_parameters = workspace_parameters msg.start_state = start_state msg.goal_constraints.append(cons) msg.num_planning_attempts = 1 msg.allowed_planning_time = 5.0 msg.group_name = joints.group_name move_group_goal.request = msg if joints.plan_only: plan_only = True move_group_goal.planning_options.plan_only = True else: plan_only = False client.send_goal(move_group_goal) client.wait_for_result(rospy.Duration.from_sec(5.0)) except rospy.ROSInterruptException, e: print "failed: %s" % e