Ask Your Question

Revision history [back]

ompl_planning/GetMotionPlan returns empty trajectories and no errors

Hi All

As per above, I am trying to call ompl_planning/GetMotionPlan service from the ompl_planning node for arm navigation.

I'm not using a move_arm action but am calling the ompl_planning node directly. I'm trying to keep it as simple as possible and just want ompl to generate a trajectory for me. I am using ROSJAVA though I dont think that should affect the message traffic.

Noting issues listed with incorrect parameters here The list of parameters I am loading in my request is here:

motionplanrequest.getMotionPlanRequest().setAllowedPlanningTime(Duration.fromMillis(5000));
                            motionplanrequest.getMotionPlanRequest().setExpectedPathDuration(Duration.fromMillis(500));
                            motionplanrequest.getMotionPlanRequest().setExpectedPathDt(Duration.fromMillis(50)); //effectively mean that the returned plan will be one interation long
                            motionplanrequest.getMotionPlanRequest().setNumPlanningAttempts(1);
                            motionplanrequest.getMotionPlanRequest().setGroupName("r_arm");
                            motionplanrequest.getMotionPlanRequest().setPlannerId("");
                            //update the GetMotionPlan Contraints with the new pose goal 
                            PositionConstraint Goalpos = node.getTopicMessageFactory().newFromType(PositionConstraint._TYPE);
                            OrientationConstraint Goalo = node.getTopicMessageFactory().newFromType(OrientationConstraint._TYPE);

                            Goalpos.getPosition().setX(desiredx);
                            Goalpos.getPosition().setY(desiredy);
                            Goalpos.getPosition().setZ(desiredz);
                            Goalpos.getHeader().setFrameId("base_link");
                            Goalpos.setLinkName("Tool");
                            double[] dims = {0.02, 0.02, 0.02};

                            Goalpos.getConstraintRegionShape().setType(Shape.BOX);
                            Goalpos.getConstraintRegionShape().setDimensions(dims);
                            Goalpos.getConstraintRegionOrientation().setW(1);
                            Goalpos.setWeight(1);

                            Goalo.getHeader().setStamp(node.getCurrentTime());
                            Goalo.getHeader().setFrameId("base_link");
                            Goalo.setLinkName("Tool");
                            Goalo.setAbsolutePitchTolerance(0.04);
                            Goalo.setAbsoluteRollTolerance(0.04);
                            Goalo.setAbsoluteYawTolerance(0.04);
                            Goalo.getOrientation().setW(desiredquat.getW());
                            Goalo.getOrientation().setX(desiredquat.getX());
                            Goalo.getOrientation().setY(desiredquat.getY());
                            Goalo.getOrientation().setZ(desiredquat.getZ());
                            Goalo.setWeight(1);

                            motionplanrequest.getMotionPlanRequest().getGoalConstraints().getPositionConstraints().add(Goalpos);
                            motionplanrequest.getMotionPlanRequest().getGoalConstraints().getOrientationConstraints().add(Goalo);

The nodes that are available are listed here:

HMM_version2_move_r_arm (move_arm/move_arm_simple_action)
 HMM_version2_r_arm_kinematics (arm_kinematics_constraint_aware/arm_kinematics_constraint_aware)
environment_server (planning_environment/environment_server)
interpolated_ik_node_left (interpolated_ik_motion_planner/interpolated_ik_motion_planner.py)
interpolated_ik_node_right (interpolated_ik_motion_planner/interpolated_ik_motion_planner.py)
mongo (mongodb/wrapper.py)
ompl_planning (ompl_ros_interface/ompl_ros)
planning_scene_validity_server (planning_environment/planning_scene_validity_server)
rob_st_pub (robot_state_publisher/state_publisher)
rviz_warehouse_viewer (rviz/rviz)
trajectory_filter_server (trajectory_filter_server/trajectory_filter_server)

plus I have my ROSJAVA node that is trying to call ompl_planning/GetMotionPlan. When It has called the service is immediately publishes the trajectory to my trajectory controller node (this works with properly formatted trajectories). I believe the trajectory is empty because the topic contains the following messages that would have been the result of the service response:

   header: 
  seq: 20046
  stamp: 
    secs: 0
    nsecs: 0
  frame_id: ''
joint_names: ['joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7']
points: []

As for other errors, there are none listed, I did have issues with a non-available planning scene however the HMM_version2_move_r_arm node cleared that up. Additionally Rviz shows my custom arm moving as it does in gazebo.

can anyone see any errors in my setup that would cause ompl_planning/getmotionplan to fail or am i missing a node?

cheers Peter

ompl_planning/GetMotionPlan returns empty trajectories and no errors

Hi All

As per above, I am trying to call ompl_planning/GetMotionPlan service from the ompl_planning node for arm navigation.

I'm not using a move_arm action but am calling the ompl_planning node directly. I'm trying to keep it as simple as possible and just want ompl to generate a trajectory for me. I am using ROSJAVA though I dont think that should affect the message traffic.

Noting issues listed with incorrect parameters here The list of parameters I am loading in my request is here:

motionplanrequest.getMotionPlanRequest().setAllowedPlanningTime(Duration.fromMillis(5000));
                            motionplanrequest.getMotionPlanRequest().setExpectedPathDuration(Duration.fromMillis(500));
                            motionplanrequest.getMotionPlanRequest().setExpectedPathDt(Duration.fromMillis(50)); //effectively mean that the returned plan will be one interation long
                            motionplanrequest.getMotionPlanRequest().setNumPlanningAttempts(1);
                            motionplanrequest.getMotionPlanRequest().setGroupName("r_arm");
                            motionplanrequest.getMotionPlanRequest().setPlannerId("");
                            //update the GetMotionPlan Contraints with the new pose goal 
                            PositionConstraint Goalpos = node.getTopicMessageFactory().newFromType(PositionConstraint._TYPE);
                            OrientationConstraint Goalo = node.getTopicMessageFactory().newFromType(OrientationConstraint._TYPE);

                            Goalpos.getPosition().setX(desiredx);
                            Goalpos.getPosition().setY(desiredy);
                            Goalpos.getPosition().setZ(desiredz);
                            Goalpos.getHeader().setFrameId("base_link");
                            Goalpos.setLinkName("Tool");
                            double[] dims = {0.02, 0.02, 0.02};

                            Goalpos.getConstraintRegionShape().setType(Shape.BOX);
                            Goalpos.getConstraintRegionShape().setDimensions(dims);
                            Goalpos.getConstraintRegionOrientation().setW(1);
                            Goalpos.setWeight(1);

                            Goalo.getHeader().setStamp(node.getCurrentTime());
                            Goalo.getHeader().setFrameId("base_link");
                            Goalo.setLinkName("Tool");
                            Goalo.setAbsolutePitchTolerance(0.04);
                            Goalo.setAbsoluteRollTolerance(0.04);
                            Goalo.setAbsoluteYawTolerance(0.04);
                            Goalo.getOrientation().setW(desiredquat.getW());
                            Goalo.getOrientation().setX(desiredquat.getX());
                            Goalo.getOrientation().setY(desiredquat.getY());
                            Goalo.getOrientation().setZ(desiredquat.getZ());
                            Goalo.setWeight(1);

                            motionplanrequest.getMotionPlanRequest().getGoalConstraints().getPositionConstraints().add(Goalpos);
                            motionplanrequest.getMotionPlanRequest().getGoalConstraints().getOrientationConstraints().add(Goalo);

The nodes that are available are listed here:here, they are basically those available from the warehouse viewer wizard, minus the warehouse viewer itself:

HMM_version2_move_r_arm (move_arm/move_arm_simple_action)
 HMM_version2_r_arm_kinematics (arm_kinematics_constraint_aware/arm_kinematics_constraint_aware)
environment_server (planning_environment/environment_server)
interpolated_ik_node_left (interpolated_ik_motion_planner/interpolated_ik_motion_planner.py)
interpolated_ik_node_right (interpolated_ik_motion_planner/interpolated_ik_motion_planner.py)
mongo (mongodb/wrapper.py)
ompl_planning (ompl_ros_interface/ompl_ros)
planning_scene_validity_server (planning_environment/planning_scene_validity_server)
rob_st_pub (robot_state_publisher/state_publisher)
rviz_warehouse_viewer (rviz/rviz)
trajectory_filter_server (trajectory_filter_server/trajectory_filter_server)

plus I have my ROSJAVA node that is trying to call ompl_planning/GetMotionPlan. When It has called the service is immediately publishes the trajectory to my trajectory controller node (this works with properly formatted trajectories). I believe the trajectory is empty because the topic contains the following messages that would have been the result of the service response:

   header: 
  seq: 20046
  stamp: 
    secs: 0
    nsecs: 0
  frame_id: ''
joint_names: ['joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7']
points: []

As for other errors, there are none listed, I did have issues with a non-available planning scene however the HMM_version2_move_r_arm node cleared that up. Additionally Rviz shows my custom arm moving as it does in gazebo.

can anyone see any errors in my setup that would cause ompl_planning/getmotionplan to fail or am i missing a node?

cheers Peter

click to hide/show revision 3
added command line list of ROS_INFO messages

ompl_planning/GetMotionPlan returns empty trajectories and no errors

Hi All

As per above, I am trying to call ompl_planning/GetMotionPlan service from the ompl_planning node for arm navigation.

I'm not using a move_arm action but am calling the ompl_planning node directly. I'm trying to keep it as simple as possible and just want ompl to generate a trajectory for me. I am using ROSJAVA though I dont think that should affect the message traffic.

Noting issues listed with incorrect parameters here The list of parameters I am loading in my request is here:

motionplanrequest.getMotionPlanRequest().setAllowedPlanningTime(Duration.fromMillis(5000));
                            motionplanrequest.getMotionPlanRequest().setExpectedPathDuration(Duration.fromMillis(500));
                            motionplanrequest.getMotionPlanRequest().setExpectedPathDt(Duration.fromMillis(50)); //effectively mean that the returned plan will be one interation long
                            motionplanrequest.getMotionPlanRequest().setNumPlanningAttempts(1);
                            motionplanrequest.getMotionPlanRequest().setGroupName("r_arm");
                            motionplanrequest.getMotionPlanRequest().setPlannerId("");
                            //update the GetMotionPlan Contraints with the new pose goal 
                            PositionConstraint Goalpos = node.getTopicMessageFactory().newFromType(PositionConstraint._TYPE);
                            OrientationConstraint Goalo = node.getTopicMessageFactory().newFromType(OrientationConstraint._TYPE);

                            Goalpos.getPosition().setX(desiredx);
                            Goalpos.getPosition().setY(desiredy);
                            Goalpos.getPosition().setZ(desiredz);
                            Goalpos.getHeader().setFrameId("base_link");
                            Goalpos.setLinkName("Tool");
                            double[] dims = {0.02, 0.02, 0.02};

                            Goalpos.getConstraintRegionShape().setType(Shape.BOX);
                            Goalpos.getConstraintRegionShape().setDimensions(dims);
                            Goalpos.getConstraintRegionOrientation().setW(1);
                            Goalpos.setWeight(1);

                            Goalo.getHeader().setStamp(node.getCurrentTime());
                            Goalo.getHeader().setFrameId("base_link");
                            Goalo.setLinkName("Tool");
                            Goalo.setAbsolutePitchTolerance(0.04);
                            Goalo.setAbsoluteRollTolerance(0.04);
                            Goalo.setAbsoluteYawTolerance(0.04);
                            Goalo.getOrientation().setW(desiredquat.getW());
                            Goalo.getOrientation().setX(desiredquat.getX());
                            Goalo.getOrientation().setY(desiredquat.getY());
                            Goalo.getOrientation().setZ(desiredquat.getZ());
                            Goalo.setWeight(1);

                            motionplanrequest.getMotionPlanRequest().getGoalConstraints().getPositionConstraints().add(Goalpos);
                            motionplanrequest.getMotionPlanRequest().getGoalConstraints().getOrientationConstraints().add(Goalo);

The nodes that are available are listed here, they are basically those available from the warehouse viewer wizard, minus the warehouse viewer itself:

HMM_version2_move_r_arm (move_arm/move_arm_simple_action)
 HMM_version2_r_arm_kinematics (arm_kinematics_constraint_aware/arm_kinematics_constraint_aware)
environment_server (planning_environment/environment_server)
interpolated_ik_node_left (interpolated_ik_motion_planner/interpolated_ik_motion_planner.py)
interpolated_ik_node_right (interpolated_ik_motion_planner/interpolated_ik_motion_planner.py)
mongo (mongodb/wrapper.py)
ompl_planning (ompl_ros_interface/ompl_ros)
planning_scene_validity_server (planning_environment/planning_scene_validity_server)
rob_st_pub (robot_state_publisher/state_publisher)
rviz_warehouse_viewer (rviz/rviz)
trajectory_filter_server (trajectory_filter_server/trajectory_filter_server)

plus I have my ROSJAVA node that is trying to call ompl_planning/GetMotionPlan. When It has called the service is immediately publishes the trajectory to my trajectory controller node (this works with properly formatted trajectories). I believe the trajectory is empty because the topic contains the following messages that would have been the result of the service response:

   header: 
  seq: 20046
  stamp: 
    secs: 0
    nsecs: 0
  frame_id: ''
joint_names: ['joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7']
points: []

As for other errors, there are none listed, I did have issues with a non-available planning scene however the HMM_version2_move_r_arm node cleared that up. Additionally Rviz shows my custom arm moving as it does in gazebo.gazebo. Below is the command output from the launch file that handles the majority of arm_navigation_nodes:

 [INFO] [WallTime: 1363504840.808273] [2221.336500] ik_utilities: waiting for IK services to be there
[INFO] [WallTime: 1363504840.853969] [2221.339800] ik_utilities: waiting for IK services to be there
[ INFO] [1363504845.675942901, 2222.123500000]: waitForService: Service [/register_planning_scene] has not been advertised, waiting...
[ INFO] [1363504845.695581190, 2222.128200000]: Environment server started
[ INFO] [1363504845.785783314, 2222.143600000]: waitForService: Service [/register_planning_scene] is now available.
[ INFO] [1363504845.960661270, 2222.176900000]: Successfully connected to planning scene action server for /ompl_planning
[ INFO] [1363504846.203129655, 2222.223800000]: Successfully connected to planning scene action server for /planning_scene_validity_server
[ INFO] [1363504846.464188408, 2222.280400000]: Successfully connected to planning scene action server for /HMM_version2_r_arm_kinematics
[ INFO] [1363504849.707711482, 2223.035700000]: Successfully connected to planning scene action server for /trajectory_filter_server

can anyone see any errors in my setup that would cause ompl_planning/getmotionplan to fail or am i missing a node?

cheers Peter

ompl_planning/GetMotionPlan returns empty trajectories and no errorsnon-valid trajectories

Hi All

As per above, I am trying to call ompl_planning/GetMotionPlan service from the ompl_planning node for arm navigation.

I'm not using a move_arm action but am calling the ompl_planning node directly. I'm trying to keep it as simple as possible and just want ompl to generate a trajectory for me. I am using ROSJAVA though I dont think that should affect the message traffic.

Noting issues listed with incorrect parameters here The list of parameters I am loading in my request is here:

motionplanrequest.getMotionPlanRequest().setAllowedPlanningTime(Duration.fromMillis(5000));
                            motionplanrequest.getMotionPlanRequest().setExpectedPathDuration(Duration.fromMillis(500));
                            motionplanrequest.getMotionPlanRequest().setExpectedPathDt(Duration.fromMillis(50)); //effectively mean that the returned plan will be one interation long
                            motionplanrequest.getMotionPlanRequest().setNumPlanningAttempts(1);
                            motionplanrequest.getMotionPlanRequest().setGroupName("r_arm");
                            motionplanrequest.getMotionPlanRequest().setPlannerId("");
                            //update the GetMotionPlan Contraints with the new pose goal 
                            PositionConstraint Goalpos = node.getTopicMessageFactory().newFromType(PositionConstraint._TYPE);
                            OrientationConstraint Goalo = node.getTopicMessageFactory().newFromType(OrientationConstraint._TYPE);

                            Goalpos.getPosition().setX(desiredx);
                            Goalpos.getPosition().setY(desiredy);
                            Goalpos.getPosition().setZ(desiredz);
                            Goalpos.getHeader().setFrameId("base_link");
                            Goalpos.setLinkName("Tool");
                            double[] dims = {0.02, 0.02, 0.02};

                            Goalpos.getConstraintRegionShape().setType(Shape.BOX);
                            Goalpos.getConstraintRegionShape().setDimensions(dims);
                            Goalpos.getConstraintRegionOrientation().setW(1);
                            Goalpos.setWeight(1);

                            Goalo.getHeader().setStamp(node.getCurrentTime());
                            Goalo.getHeader().setFrameId("base_link");
                            Goalo.setLinkName("Tool");
                            Goalo.setAbsolutePitchTolerance(0.04);
                            Goalo.setAbsoluteRollTolerance(0.04);
                            Goalo.setAbsoluteYawTolerance(0.04);
                            Goalo.getOrientation().setW(desiredquat.getW());
                            Goalo.getOrientation().setX(desiredquat.getX());
                            Goalo.getOrientation().setY(desiredquat.getY());
                            Goalo.getOrientation().setZ(desiredquat.getZ());
                            Goalo.setWeight(1);

                            motionplanrequest.getMotionPlanRequest().getGoalConstraints().getPositionConstraints().add(Goalpos);
                            motionplanrequest.getMotionPlanRequest().getGoalConstraints().getOrientationConstraints().add(Goalo);

The nodes that are available are listed here, they are basically those available from the warehouse viewer wizard, minus the warehouse viewer itself:

HMM_version2_move_r_arm (move_arm/move_arm_simple_action)
 HMM_version2_r_arm_kinematics (arm_kinematics_constraint_aware/arm_kinematics_constraint_aware)
environment_server (planning_environment/environment_server)
interpolated_ik_node_left (interpolated_ik_motion_planner/interpolated_ik_motion_planner.py)
interpolated_ik_node_right (interpolated_ik_motion_planner/interpolated_ik_motion_planner.py)
mongo (mongodb/wrapper.py)
ompl_planning (ompl_ros_interface/ompl_ros)
planning_scene_validity_server (planning_environment/planning_scene_validity_server)
rob_st_pub (robot_state_publisher/state_publisher)
rviz_warehouse_viewer (rviz/rviz)
trajectory_filter_server (trajectory_filter_server/trajectory_filter_server)

plus I have my ROSJAVA node that is trying to call ompl_planning/GetMotionPlan. When It has called the service is immediately publishes the trajectory to my trajectory controller node (this works with properly formatted trajectories). I believe the trajectory is empty because the topic contains the following messages that would have been the result of the service response:

   header: 
  seq: 20046
  stamp: 
    secs: 0
    nsecs: 0
  frame_id: ''
joint_names: ['joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7']
points: []

As for other errors, there are none listed, I did have issues with a non-available planning scene however the HMM_version2_move_r_arm node cleared that up. Additionally Rviz shows my custom arm moving as it does in gazebo. Below is the command output from the launch file that handles the majority of arm_navigation_nodes:

 [INFO] [WallTime: 1363504840.808273] [2221.336500] ik_utilities: waiting for IK services to be there
[INFO] [WallTime: 1363504840.853969] [2221.339800] ik_utilities: waiting for IK services to be there
[ INFO] [1363504845.675942901, 2222.123500000]: waitForService: Service [/register_planning_scene] has not been advertised, waiting...
[ INFO] [1363504845.695581190, 2222.128200000]: Environment server started
[ INFO] [1363504845.785783314, 2222.143600000]: waitForService: Service [/register_planning_scene] is now available.
[ INFO] [1363504845.960661270, 2222.176900000]: Successfully connected to planning scene action server for /ompl_planning
[ INFO] [1363504846.203129655, 2222.223800000]: Successfully connected to planning scene action server for /planning_scene_validity_server
[ INFO] [1363504846.464188408, 2222.280400000]: Successfully connected to planning scene action server for /HMM_version2_r_arm_kinematics
[ INFO] [1363504849.707711482, 2223.035700000]: Successfully connected to planning scene action server for /trajectory_filter_server

the only error code I can find is the one in the message but it gives a non valid error of 0, which could be just an empty msg. Can anyone see any errors in my setup that would cause ompl_planning/getmotionplan to fail or am i missing a node?

cheers Peter

ompl_planning/GetMotionPlan returns non-valid trajectories

Hi All

As per above, I am trying to call ompl_planning/GetMotionPlan service from the ompl_planning node for arm navigation.

I'm not using a move_arm action but am calling the ompl_planning node directly. I'm trying to keep it as simple as possible and just want ompl to generate a trajectory for me. I am using ROSJAVA though I dont think that should affect the message traffic.

Noting issues listed with incorrect parameters here The list of parameters I am loading in my request is here:

motionplanrequest.getMotionPlanRequest().setAllowedPlanningTime(Duration.fromMillis(5000));
                            motionplanrequest.getMotionPlanRequest().setExpectedPathDuration(Duration.fromMillis(500));
                            motionplanrequest.getMotionPlanRequest().setExpectedPathDt(Duration.fromMillis(50)); //effectively mean that the returned plan will be one interation long
                            motionplanrequest.getMotionPlanRequest().setNumPlanningAttempts(1);
                            motionplanrequest.getMotionPlanRequest().setGroupName("r_arm");
                            motionplanrequest.getMotionPlanRequest().setPlannerId("");
                            //update the GetMotionPlan Contraints with the new pose goal 
                            PositionConstraint Goalpos = node.getTopicMessageFactory().newFromType(PositionConstraint._TYPE);
                            OrientationConstraint Goalo = node.getTopicMessageFactory().newFromType(OrientationConstraint._TYPE);

                            Goalpos.getPosition().setX(desiredx);
                            Goalpos.getPosition().setY(desiredy);
                            Goalpos.getPosition().setZ(desiredz);
                            Goalpos.getHeader().setFrameId("base_link");
                            Goalpos.setLinkName("Tool");
                            double[] dims = {0.02, 0.02, 0.02};

                            Goalpos.getConstraintRegionShape().setType(Shape.BOX);
                            Goalpos.getConstraintRegionShape().setDimensions(dims);
                            Goalpos.getConstraintRegionOrientation().setW(1);
                            Goalpos.setWeight(1);

                            Goalo.getHeader().setStamp(node.getCurrentTime());
                            Goalo.getHeader().setFrameId("base_link");
                            Goalo.setLinkName("Tool");
                            Goalo.setAbsolutePitchTolerance(0.04);
                            Goalo.setAbsoluteRollTolerance(0.04);
                            Goalo.setAbsoluteYawTolerance(0.04);
                            Goalo.getOrientation().setW(desiredquat.getW());
                            Goalo.getOrientation().setX(desiredquat.getX());
                            Goalo.getOrientation().setY(desiredquat.getY());
                            Goalo.getOrientation().setZ(desiredquat.getZ());
                            Goalo.setWeight(1);

                            motionplanrequest.getMotionPlanRequest().getGoalConstraints().getPositionConstraints().add(Goalpos);
                            motionplanrequest.getMotionPlanRequest().getGoalConstraints().getOrientationConstraints().add(Goalo);

The nodes that are available are listed here, they are basically those available from the warehouse viewer wizard, minus the warehouse viewer itself:

HMM_version2_move_r_arm (move_arm/move_arm_simple_action)
 HMM_version2_r_arm_kinematics (arm_kinematics_constraint_aware/arm_kinematics_constraint_aware)
environment_server (planning_environment/environment_server)
interpolated_ik_node_left (interpolated_ik_motion_planner/interpolated_ik_motion_planner.py)
interpolated_ik_node_right (interpolated_ik_motion_planner/interpolated_ik_motion_planner.py)
mongo (mongodb/wrapper.py)
ompl_planning (ompl_ros_interface/ompl_ros)
planning_scene_validity_server (planning_environment/planning_scene_validity_server)
rob_st_pub (robot_state_publisher/state_publisher)
rviz_warehouse_viewer (rviz/rviz)
trajectory_filter_server (trajectory_filter_server/trajectory_filter_server)

plus I have my ROSJAVA node that is trying to call ompl_planning/GetMotionPlan. When It has called the service is immediately publishes the trajectory to my trajectory controller node (this works with properly formatted trajectories). I believe the trajectory is empty because the topic contains the following messages that would have been the result of the service response:

   header: 
  seq: 20046
  stamp: 
    secs: 0
    nsecs: 0
  frame_id: ''
joint_names: ['joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7']
points: []

As for other errors, there are none listed, I did have issues with a non-available planning scene however the HMM_version2_move_r_arm node cleared that up. Additionally Rviz shows my custom arm moving as it does in gazebo. Below is the command output from the launch file that handles the majority of arm_navigation_nodes:

 [INFO] [WallTime: 1363504840.808273] [2221.336500] ik_utilities: waiting for IK services to be there
[INFO] [WallTime: 1363504840.853969] [2221.339800] ik_utilities: waiting for IK services to be there
[ INFO] [1363504845.675942901, 2222.123500000]: waitForService: Service [/register_planning_scene] has not been advertised, waiting...
[ INFO] [1363504845.695581190, 2222.128200000]: Environment server started
[ INFO] [1363504845.785783314, 2222.143600000]: waitForService: Service [/register_planning_scene] is now available.
[ INFO] [1363504845.960661270, 2222.176900000]: Successfully connected to planning scene action server for /ompl_planning
[ INFO] [1363504846.203129655, 2222.223800000]: Successfully connected to planning scene action server for /planning_scene_validity_server
[ INFO] [1363504846.464188408, 2222.280400000]: Successfully connected to planning scene action server for /HMM_version2_r_arm_kinematics
[ INFO] [1363504849.707711482, 2223.035700000]: Successfully connected to planning scene action server for /trajectory_filter_server

the only error code I can find is the one in the message but it gives a non valid error of 0, which could be just an empty msg. Can anyone see any errors in my setup that would cause ompl_planning/getmotionplan to fail or am i missing a node?

cheers Peter

ompl_planning/GetMotionPlan returns non-valid trajectories

Hi All

As per above, I am trying to call ompl_planning/GetMotionPlan service from the ompl_planning node for arm navigation.

I'm not using a move_arm action but am calling the ompl_planning node directly. I'm trying to keep it as simple as possible and just want ompl to generate a trajectory for me. I am using ROSJAVA though I dont think that should affect the message traffic.

Noting issues listed with incorrect parameters here The list of parameters I am loading in my request is here:

motionplanrequest.getMotionPlanRequest().setAllowedPlanningTime(Duration.fromMillis(5000));
                            motionplanrequest.getMotionPlanRequest().setExpectedPathDuration(Duration.fromMillis(500));
                            motionplanrequest.getMotionPlanRequest().setExpectedPathDt(Duration.fromMillis(50)); //effectively mean that the returned plan will be one interation long
                            motionplanrequest.getMotionPlanRequest().setNumPlanningAttempts(1);
                            motionplanrequest.getMotionPlanRequest().setGroupName("r_arm");
                            motionplanrequest.getMotionPlanRequest().setPlannerId("");
                            //update the GetMotionPlan Contraints with the new pose goal 
                            PositionConstraint Goalpos = node.getTopicMessageFactory().newFromType(PositionConstraint._TYPE);
                            OrientationConstraint Goalo = node.getTopicMessageFactory().newFromType(OrientationConstraint._TYPE);

                            Goalpos.getPosition().setX(desiredx);
                            Goalpos.getPosition().setY(desiredy);
                            Goalpos.getPosition().setZ(desiredz);
                            Goalpos.getHeader().setFrameId("base_link");
                            Goalpos.setLinkName("Tool");
                            double[] dims = {0.02, 0.02, 0.02};

                            Goalpos.getConstraintRegionShape().setType(Shape.BOX);
                            Goalpos.getConstraintRegionShape().setDimensions(dims);
                            Goalpos.getConstraintRegionOrientation().setW(1);
                            Goalpos.setWeight(1);

                            Goalo.getHeader().setStamp(node.getCurrentTime());
                            Goalo.getHeader().setFrameId("base_link");
                            Goalo.setLinkName("Tool");
                            Goalo.setAbsolutePitchTolerance(0.04);
                            Goalo.setAbsoluteRollTolerance(0.04);
                            Goalo.setAbsoluteYawTolerance(0.04);
                            Goalo.getOrientation().setW(desiredquat.getW());
                            Goalo.getOrientation().setX(desiredquat.getX());
                            Goalo.getOrientation().setY(desiredquat.getY());
                            Goalo.getOrientation().setZ(desiredquat.getZ());
                            Goalo.setWeight(1);

                            motionplanrequest.getMotionPlanRequest().getGoalConstraints().getPositionConstraints().add(Goalpos);
                            motionplanrequest.getMotionPlanRequest().getGoalConstraints().getOrientationConstraints().add(Goalo);

The nodes that are available are listed here, they are basically those available from the warehouse viewer wizard, minus the warehouse viewer itself:

HMM_version2_move_r_arm (move_arm/move_arm_simple_action)
 HMM_version2_r_arm_kinematics (arm_kinematics_constraint_aware/arm_kinematics_constraint_aware)
environment_server (planning_environment/environment_server)
interpolated_ik_node_left (interpolated_ik_motion_planner/interpolated_ik_motion_planner.py)
interpolated_ik_node_right (interpolated_ik_motion_planner/interpolated_ik_motion_planner.py)
mongo (mongodb/wrapper.py)
ompl_planning (ompl_ros_interface/ompl_ros)
planning_scene_validity_server (planning_environment/planning_scene_validity_server)
rob_st_pub (robot_state_publisher/state_publisher)
rviz_warehouse_viewer (rviz/rviz)
trajectory_filter_server (trajectory_filter_server/trajectory_filter_server)

plus I have my ROSJAVA node that is trying to call ompl_planning/GetMotionPlan. When It has called the service is immediately publishes the trajectory to my trajectory controller node (this works with properly formatted trajectories). I believe the trajectory is empty because the topic contains the following messages that would have been the result of the service response:

   header: 
  seq: 20046
  stamp: 
    secs: 0
    nsecs: 0
  frame_id: ''
joint_names: ['joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7']
points: []

As for other errors, there are none listed, I did have issues with a non-available planning scene however the HMM_version2_move_r_arm node cleared that up. Additionally Rviz shows my custom arm moving as it does in gazebo. Below is the command output from the launch file that handles the majority of arm_navigation_nodes:

 [INFO] [WallTime: 1363504840.808273] [2221.336500] ik_utilities: waiting for IK services to be there
[INFO] [WallTime: 1363504840.853969] [2221.339800] ik_utilities: waiting for IK services to be there
[ INFO] [1363504845.675942901, 2222.123500000]: waitForService: Service [/register_planning_scene] has not been advertised, waiting...
[ INFO] [1363504845.695581190, 2222.128200000]: Environment server started
[ INFO] [1363504845.785783314, 2222.143600000]: waitForService: Service [/register_planning_scene] is now available.
[ INFO] [1363504845.960661270, 2222.176900000]: Successfully connected to planning scene action server for /ompl_planning
[ INFO] [1363504846.203129655, 2222.223800000]: Successfully connected to planning scene action server for /planning_scene_validity_server
[ INFO] [1363504846.464188408, 2222.280400000]: Successfully connected to planning scene action server for /HMM_version2_r_arm_kinematics
[ INFO] [1363504849.707711482, 2223.035700000]: Successfully connected to planning scene action server for /trajectory_filter_server

the only error code I can find is the one in the message but it gives a non valid error of 0, which could be just an empty msg. Can anyone see any errors in my setup that would cause ompl_planning/getmotionplan to fail or am i missing a node?

cheers Peter

ompl_planning/GetMotionPlan returns non-valid trajectories

Hi All

As per above, I am trying to call ompl_planning/GetMotionPlan service from the ompl_planning node for arm navigation.

I'm not using a move_arm action but am calling the ompl_planning node directly. I'm trying to keep it as simple as possible and just want ompl to generate a trajectory for me. I am using ROSJAVA though I dont think that should affect the message traffic.

Noting issues listed with incorrect parameters here The list of parameters I am loading in my request is here:

motionplanrequest.getMotionPlanRequest().setAllowedPlanningTime(Duration.fromMillis(5000));
                            motionplanrequest.getMotionPlanRequest().setExpectedPathDuration(Duration.fromMillis(500));
                            motionplanrequest.getMotionPlanRequest().setExpectedPathDt(Duration.fromMillis(50)); //effectively mean that the returned plan will be one interation long
                            motionplanrequest.getMotionPlanRequest().setNumPlanningAttempts(1);
                            motionplanrequest.getMotionPlanRequest().setGroupName("r_arm");
                            motionplanrequest.getMotionPlanRequest().setPlannerId("");
                            //update the GetMotionPlan Contraints with the new pose goal 
                            PositionConstraint Goalpos = node.getTopicMessageFactory().newFromType(PositionConstraint._TYPE);
                            OrientationConstraint Goalo = node.getTopicMessageFactory().newFromType(OrientationConstraint._TYPE);

                            Goalpos.getPosition().setX(desiredx);
                            Goalpos.getPosition().setY(desiredy);
                            Goalpos.getPosition().setZ(desiredz);
                            Goalpos.getHeader().setFrameId("base_link");
                            Goalpos.setLinkName("Tool");
                            double[] dims = {0.02, 0.02, 0.02};

                            Goalpos.getConstraintRegionShape().setType(Shape.BOX);
                            Goalpos.getConstraintRegionShape().setDimensions(dims);
                            Goalpos.getConstraintRegionOrientation().setW(1);
                            Goalpos.setWeight(1);

                            Goalo.getHeader().setStamp(node.getCurrentTime());
                            Goalo.getHeader().setFrameId("base_link");
                            Goalo.setLinkName("Tool");
                            Goalo.setAbsolutePitchTolerance(0.04);
                            Goalo.setAbsoluteRollTolerance(0.04);
                            Goalo.setAbsoluteYawTolerance(0.04);
                            Goalo.getOrientation().setW(desiredquat.getW());
                            Goalo.getOrientation().setX(desiredquat.getX());
                            Goalo.getOrientation().setY(desiredquat.getY());
                            Goalo.getOrientation().setZ(desiredquat.getZ());
                            Goalo.setWeight(1);

                            motionplanrequest.getMotionPlanRequest().getGoalConstraints().getPositionConstraints().add(Goalpos);
                            motionplanrequest.getMotionPlanRequest().getGoalConstraints().getOrientationConstraints().add(Goalo);

The nodes that are available are listed here, they are basically those available from the warehouse viewer wizard, minus the warehouse viewer itself:

HMM_version2_move_r_arm (move_arm/move_arm_simple_action)
 HMM_version2_r_arm_kinematics (arm_kinematics_constraint_aware/arm_kinematics_constraint_aware)
environment_server (planning_environment/environment_server)
interpolated_ik_node_left (interpolated_ik_motion_planner/interpolated_ik_motion_planner.py)
interpolated_ik_node_right (interpolated_ik_motion_planner/interpolated_ik_motion_planner.py)
mongo (mongodb/wrapper.py)
ompl_planning (ompl_ros_interface/ompl_ros)
planning_scene_validity_server (planning_environment/planning_scene_validity_server)
rob_st_pub (robot_state_publisher/state_publisher)
rviz_warehouse_viewer (rviz/rviz)
trajectory_filter_server (trajectory_filter_server/trajectory_filter_server)

plus I have my ROSJAVA node that is trying to call ompl_planning/GetMotionPlan. When It has called the service is immediately publishes the trajectory to my trajectory controller node (this works with properly formatted trajectories). I believe the trajectory is empty because the topic contains the following messages that would have been the result of the service response:

   header: 
  seq: 20046
  stamp: 
    secs: 0
    nsecs: 0
  frame_id: ''
joint_names: ['joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7']
points: []

As for other errors, there are none listed, I did have issues with a non-available planning scene however the HMM_version2_move_r_arm node cleared that up. Additionally Rviz shows my custom arm moving as it does in gazebo. Below is the command output from the launch file that handles the majority of arm_navigation_nodes:

 [INFO] [WallTime: 1363504840.808273] [2221.336500] ik_utilities: waiting for IK services to be there
[INFO] [WallTime: 1363504840.853969] [2221.339800] ik_utilities: waiting for IK services to be there
[ INFO] [1363504845.675942901, 2222.123500000]: waitForService: Service [/register_planning_scene] has not been advertised, waiting...
[ INFO] [1363504845.695581190, 2222.128200000]: Environment server started
[ INFO] [1363504845.785783314, 2222.143600000]: waitForService: Service [/register_planning_scene] is now available.
[ INFO] [1363504845.960661270, 2222.176900000]: Successfully connected to planning scene action server for /ompl_planning
[ INFO] [1363504846.203129655, 2222.223800000]: Successfully connected to planning scene action server for /planning_scene_validity_server
[ INFO] [1363504846.464188408, 2222.280400000]: Successfully connected to planning scene action server for /HMM_version2_r_arm_kinematics
[ INFO] [1363504849.707711482, 2223.035700000]: Successfully connected to planning scene action server for /trajectory_filter_server

the only error code I can find is the one in the message but it gives a non valid error of 0, which could be just an empty msg. Can anyone see any errors in my setup that would cause ompl_planning/getmotionplan to fail or am i missing a node?

cheers Peter