Index: pr2_arm_navigation_actions/launch/environment_server_right_arm.launch =================================================================== --- pr2_arm_navigation_actions/launch/environment_server_right_arm.launch (revision 49815) +++ pr2_arm_navigation_actions/launch/environment_server_right_arm.launch (working copy) @@ -6,7 +6,7 @@ - + Index: pr2_arm_navigation_actions/launch/move_right_arm.launch =================================================================== --- pr2_arm_navigation_actions/launch/move_right_arm.launch (revision 49815) +++ pr2_arm_navigation_actions/launch/move_right_arm.launch (working copy) @@ -14,10 +14,9 @@ - + - Index: pr2_arm_navigation_config/config/planning_groups.yaml =================================================================== --- pr2_arm_navigation_config/config/planning_groups.yaml (revision 49815) +++ pr2_arm_navigation_config/config/planning_groups.yaml (working copy) @@ -55,6 +55,23 @@ r_forearm_cam_frame r_wrist_flex_link r_wrist_roll_link + + - name: right_arm_planar + joints: + r_shoulder_pan_joint + r_elbow_flex_joint + r_wrist_flex_joint + links: + r_shoulder_pan_link + r_shoulder_lift_link + r_upper_arm_link + r_upper_arm_roll_link + r_elbow_flex_link + r_forearm_link + r_forearm_roll_link + r_forearm_cam_frame + r_wrist_flex_link + r_wrist_roll_link - name: arms joints: Index: pr2_arm_navigation_planning/config/ompl_planning_configs.yaml =================================================================== --- pr2_arm_navigation_planning/config/ompl_planning_configs.yaml (revision 49815) +++ pr2_arm_navigation_planning/config/ompl_planning_configs.yaml (working copy) @@ -2,7 +2,7 @@ ## the list of groups for which motion planning can be performed group_list: - right_arm left_arm + right_arm left_arm right_arm_planar ## the planner configurations; each config must have a type, which specifies ## the planner to be used; other parameters can be specified as well, depending @@ -100,6 +100,10 @@ - name: right_arm planner_configs: RRTkConfig2 SBLkConfig2 KPIECEkConfig1 KPIECEkConfig2r LBKPIECEkConfig2r armIKConfig1 + + - name: right_arm_planar + planner_configs: + RRTkConfig2 SBLkConfig2 KPIECEkConfig1 KPIECEkConfig2r LBKPIECEkConfig2r armIKConfig1 - name: arms planner_configs: Index: pr2_arm_navigation_tutorials/src/move_arm_joint_goal.cpp =================================================================== --- pr2_arm_navigation_tutorials/src/move_arm_joint_goal.cpp (revision 49815) +++ pr2_arm_navigation_tutorials/src/move_arm_joint_goal.cpp (working copy) @@ -5,22 +5,18 @@ int main(int argc, char **argv){ ros::init (argc, argv, "move_arm_joint_goal_test"); ros::NodeHandle nh; - actionlib::SimpleActionClient move_arm("move_right_arm",true); + actionlib::SimpleActionClient move_arm("move_right_arm_planar",true); move_arm.waitForServer(); ROS_INFO("Connected to server"); move_arm_msgs::MoveArmGoal goalB; - std::vector names(7); + std::vector names(3); names[0] = "r_shoulder_pan_joint"; - names[1] = "r_shoulder_lift_joint"; - names[2] = "r_upper_arm_roll_joint"; - names[3] = "r_elbow_flex_joint"; - names[4] = "r_forearm_roll_joint"; - names[5] = "r_wrist_flex_joint"; - names[6] = "r_wrist_roll_joint"; + names[1] = "r_elbow_flex_joint"; + names[2] = "r_wrist_flex_joint"; - goalB.motion_plan_request.group_name = "right_arm"; + goalB.motion_plan_request.group_name = "right_arm_planar"; goalB.motion_plan_request.num_planning_attempts = 1; goalB.motion_plan_request.allowed_planning_time = ros::Duration(5.0); @@ -37,8 +33,8 @@ } goalB.motion_plan_request.goal_constraints.joint_constraints[0].position = -2.0; - goalB.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2; - goalB.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.15; + goalB.motion_plan_request.goal_constraints.joint_constraints[1].position = -0.2; + goalB.motion_plan_request.goal_constraints.joint_constraints[2].position = -0.15; if (nh.ok()) {