STOMP planner and moveit

1,954 views
Skip to first unread message

Teresa Seco

unread,
Mar 4, 2016, 5:32:14 AM3/4/16
to swri-ros-pkg-dev
Hello,

I am working with Gazebo and UR10 robot model. There is an asus camera in the enviroment that generates the octomap. RVIZ shows this octomap.

I put a cylinder in Gazebo between two possible positions of the robot because I want to check the planner with an without the cylinder in the scene. The octomap shows the cylinder. We have checked with different ompl planners (LBKPIECE, RRT, RRT*) (moveit) and different parameter values (num_planning_attemps is one of them) and the planners find a valid solution for the movement of the robot but it is not the most "logical" movement. The planned trajectories are quite strange.

For this reason, we have started to check STOMP planner with moveit but there is not much information about this planner. We downloaded the package from https://github.com/ros-industrial/industrial_moveit and we can move our UR10 robot (gazebo) with RVIZ using this planner but we need to adjust some parameters and try to understand how it works. We would like to know the meaning of stomp_planning.yaml pararameters for example.

We also use move_group interface (C++) with setTargetPose and asyncExecute but it does not work the STOMP planner returns this error:  Invalid goal definition, STOMP can only accept joint goals.

Any suggestions wolud be great to start!

Teresa

Levi Armstrong

unread,
Mar 4, 2016, 8:51:03 AM3/4/16
to swri-ros...@googlegroups.com
Hello Teresa,

I will see if I can help. I have included a brief description of the key parameters below. In reference to the error you are receiving, STOMP operates in joint space so it can only accept joint goals. If you only have a Cartesian pose you can use the RobotState::setFromIK to get a set of joints for the given pose and pass that to STOMP to solve the issue.
  • Num_time_steps: It’s the number of joint configurations per trajectory. The larger this number, the smoother the trajectory tends to be, however there will be more computations required to generate such trajectory.
  • Max_iterations: The maximum allowed number of iterations STOMP can perform before finding a collision free trajectory, then it will fail.
  • Num_rollouts_per_iteration: The number of noisy candidate trajectories are generated every iteration in order to find a better trajectory. More computations will be required when this number is set to a higher value.
  • Noise_coefficients: These coefficients are used to randomize the value of the joints during the generation of noisy candidate trajectories that are fed into the optimization task. The most influential parameters are “stddev” and “decay”. Each entry in these arrays corresponds to an active joint in the planning group. Setting these to higher values will allow a joint to move more rapidly, however values that are too large may generate trajectories with unnecessary large motions.
Regards,

Levi 

--
You received this message because you are subscribed to the Google Groups "swri-ros-pkg-dev" group.
To unsubscribe from this group and stop receiving emails from it, send an email to swri-ros-pkg-d...@googlegroups.com.
For more options, visit https://groups.google.com/d/optout.



--
Levi Armstrong

Teresa Seco

unread,
Mar 8, 2016, 8:04:32 AM3/8/16
to swri-ros-pkg-dev
Hello Levi,

Thank you for clarifying.

I manage to send joint goals using RobotState::setFromIK, but the STOMP planner does not work properly. The planning time is very high and the generated trajectory is a little strange.
Moreover some warnings arises:

[ INFO] [1457441684.161807710, 76.854000000]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1457441690.494294739, 83.164000000]: STOMP found a valid path after 6.33021 seconds
[ INFO] [1457441690.517254149, 83.187000000]: Received new trajectory execution service request...
[ INFO] [1457441690.517580466, 83.187000000]: Trajectory was successfully forwarded to the controller
[ WARN] [1457441690.518214687, 83.188000000]: Dropping first 1 trajectory point(s) out of 42, as they occur before the current time.
First valid point will be reached in 7.649s.
[ WARN] [1457441693.227553148, 85.888000000]: Controller  failed with error code PATH_TOLERANCE_VIOLATED
[ WARN] [1457441693.227673479, 85.888000000]: Controller handle  reports status ABORTED

The stomp_planning.yaml file is:

# Optimization Task
num_feature_basis_functions: 1
trajectory_duration: 8.0
num_time_steps: 40 #20
publish_trajectory_markers: False
publish_best_trajectory_marker: True


# STOMP
max_iterations: 10
max_iterations_after_collision_free: 2
max_rollouts: 100
min_rollouts: 10
num_rollouts_per_iteration: 20

# STOMP (optional)
use_noise_adaptation: true
write_to_file: false
use_openmp: false

noise_coefficients:
 - group_name: manipulator
   stddev: [0.3, 0.3, 0.3, 0.3, 0.3, 0.3]
   min_stddev: [0.3, 0.3, 0.3, 0.3, 0.3, 0.3]
   decay:  [2.0, 1.5, 2.0, 2.0, 2.0, 2.0]

features:
- class: stomp_moveit_interface/ObstacleAvoidanceFeature
  collision_clearance: 0.02

Any suggestions?

Another issue that we found is that using RVIZ to move the ur10 robot and adding an octomap, the planner fails... Is STOMP ready to work with octomap?

Thanks in advance,
Teresa

Jorge Nicho

unread,
Mar 9, 2016, 10:48:46 AM3/9/16
to swri-ros-pkg-dev
Hello Teresa
Make sure that the stomp_planning_pipeline.launch file contains the AddTimeParameterization planning adapter as shown below:

  <arg name="planning_adapters" value="default_planner_request_adapters/AddTimeParameterization 
                                       default_planner_request_adapters/FixWorkspaceBounds
                                       default_planner_request_adapters/FixStartStateBounds
                                       default_planner_request_adapters/FixStartStateCollision
                                       default_planner_request_adapters/FixStartStatePathConstraints" />

This tends to fix issues with timing information in the trajectory.

In regards to the octomap question, STOMP can plan around it but in our experience it is noticeable slower.  We'll work on fixing this in the upcoming weeks.

Jorge 

Teresa Seco

unread,
Mar 9, 2016, 11:38:06 AM3/9/16
to swri-ros-pkg-dev
Hello Jorge,

The stomp_planning_pipeline.launch includes the AddTimeParameterization planning adapater yet, so I do not know wich parameters I should set to try to obtain a reasolable result. Moreover, the trajectory found by the planner is a little strange with some unnecesary arm movements.

STOM seems to be the best planner up to now but it is the first time that we use it so it would be very appreciate if someone could give us some basic steps to try to configure it in order to obtain the best result.

Thanks in advance.

Jorge Nicho

unread,
Mar 9, 2016, 3:26:49 PM3/9/16
to swri-ros-pkg-dev
Teresa,
The noise coefficients need to be tuned depending on how large your work volume is.  I'm assuming that the coefficients you are using came from the stomp_test package, those were tuned for the kr210 which is a big arm with a relatively large work volume and it's mounted on a rail.  I'd suggest trying out smaller coefficients as shown below:

 - group_name: manipulator
   stddev: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
   min_stddev: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01]
   decay:  [0.999, 0.999, 0.999, 0.999, 0.999, 0.999, 0.999]

 Each element in these array apply to a specific joint, for instance , the third element from the left (index 2) will affect the behavior of the third joint in the robot's kinematic chain.  

Jorge


On Friday, March 4, 2016 at 4:32:14 AM UTC-6, Teresa Seco wrote:

Teresa Seco

unread,
Mar 10, 2016, 5:18:59 AM3/10/16
to swri-ros-pkg-dev
Thank you very much for your suggestion. The planning time has decreases and the arm movement is quite better (although the execution is slower).

I have a problem working with my code. When I plan and execute a first movement, the planner returns a trajectory and executes it from the initial position, but after this movement, if I send another joints goal, the planned trajectory consists of a first movement to the initial position (of the first movement) and then goes to the final goal, instead of going from the current pose to the final goal.

This is the C++ code:

 int UR10_collision_control::moveURtoTargetPosition(geometry_msgs::Pose targetPose, double max_vel_scaling_factor)
{
        geometry_msgs::PoseStamped currentPose;

        //joint space goal
        robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
        robot_model::RobotModelPtr robot_model_ptr = robot_model_loader.getModel();
        robot_state::RobotStatePtr robot_state_ptr(group_->getCurrentState()); //copy state
        //robot_state_ptr->setToDefaultValues();

        const robot_state::JointModelGroup* joint_model_group = robot_model_ptr->getJointModelGroup(group_->getName());
     
        bool found_ik = robot_state_ptr->setFromIK(joint_model_group, targetPose, 10, 1);

        std::vector<double> jointValues;
        robot_state_ptr->copyJointGroupPositions("manipulator", jointValues);

        ROS_INFO("joint_values: %f %f %f %f %f %f",jointValues[0],jointValues[1],jointValues[2],jointValues[3],jointValues[4],jointValues[5]);
        group_->setJointValueTarget(jointValues);

        setMaxVelScalingFactor(max_vel_scaling_factor);

        moveit::planning_interface::MoveGroup::Plan plan;
        bool success = group_->plan(plan);
        ROS_INFO("Visualizing plan  (pose goal) %s",success?"":"FAILED");

        group_->asyncExecute(plan);

        currentPose = group_->getCurrentPose("ee_link");
//    ROS_INFO("Final pose_x: %f\n current_pose_y :%f\n current_pose_z: %f",currentPose.pose.position.x,currentPose.pose.position.y, currentPose.pose.position.z);
       if (success)
          return 0;
       else
           return 1;
}

With another planner I do not have this problem.
Any suggestions?
Thank in advance.

Jorge Nicho

unread,
Mar 14, 2016, 11:35:12 AM3/14/16
to swri-ros-pkg-dev
Teresa,
Your code looks right to me.  I would try calling the MoveGroup::setStartStateToCurrentState() in order to ensure that the start state is the same as the current state.
Thanks 


On Friday, March 4, 2016 at 4:32:14 AM UTC-6, Teresa Seco wrote:

Teresa Seco

unread,
Mar 15, 2016, 3:57:07 AM3/15/16
to swri-ros-pkg-dev
Hi,

I tried with your suggestion and the result is the same... Moreover, the movement of the robot once the "weird" planned trajectory is obtained is very slow too. With RViz it seems to work better (planning time high but the execution is ok).

Thanks in advance

Teresa

Jorge Nicho

unread,
Mar 16, 2016, 10:58:09 AM3/16/16
to swri-ros-pkg-dev

Teresa,
In what sense is the trajectory "weird".  
Not sure about the issue you are having with the subsequent motion plans moving to the start position of the first plan;  all I could think of is that your true "Robot State" was never changed to begin with during the execution of the first motion plan.  I don't know much about Gazebo but It could be that Gazebo only updates it's own internal robot model but not the Robot State that the "move_group" node is tracking, this situation could help explain some of the behavior you are seeing. It be helpful if you could describe your virtual setup in more detail
Message has been deleted

Teresa Seco

unread,
Mar 16, 2016, 12:55:04 PM3/16/16
to swri-ros-pkg-dev
Hi,

I am not sure if Gazebo is the reason for this behaviour because with other planners there is no problem working with it.
We have a real UR10 arm but we wanted to ensure a proper plannification before use it...

We use Gazebo with the UR10 robot model and moveit with all the configuration files needed to work with. There is a ros node with the code described in the previous post. We use a service implemented in this node to set the target pose through rqt. The link video shows the "weird" trajectory when only the z value is changed from the second position

https://drive.google.com/folderview?id=0B1OwbapT2nrsTUgzLXA0NTlyd1U&usp=sharing

As I mentioned in the previous post, this behaviour is not observed using RVIZ to plan and execute a final goal.

I hope this could clarify the virtual scenario.

Teresa

Jorge Nicho

unread,
Mar 17, 2016, 10:04:50 AM3/17/16
to swri-ros-pkg-dev
Teresa,
It looks as if there is an error on the robot driver, this could mean that the moveit robot state isn't getting updated with the joint values of the actual robot and so this may explain why it goes back to the start of the first trajectory.  In regards to the execution time, you can tune this by changing the value in the stomp_config.yaml file.  
Message has been deleted

Teresa Seco

unread,
Mar 18, 2016, 8:55:43 AM3/18/16
to swri-ros-pkg-dev
Hi,

I checked with the real robot and the problem persisted but I found the solution!

The problem was in: group_->setStartStateToCurrentState();

Using the next code insted of the previous one, the start state is fixed to the current state:

    robot_state::RobotStatePtr robotState;
    robotState = group_->getCurrentState();
    group_->setStartState(*robotState);


Although the movement starts from the current state now, I have two problems:

1- The planning time is very high
2- There is a not desired oscillation

In the following link you can observe both issues (at the botton of console  the planning time is showed).

https://drive.google.com/file/d/0B1OwbapT2nrsWG5JNDVmSjR3dEk/view?usp=sharing

We are setting different values in stomp_planning.yaml, but it seems that they are not the best ones. Any suggestion about that would be great.

Regarding to the execution time parameter to decrese the execution time, do you refer to trajectory_duration?

Thanks in advance,

Teresa

Jorge Nicho

unread,
Mar 18, 2016, 4:43:38 PM3/18/16
to swri-ros-pkg-dev
Teresa,
I'm glad you were able to solve that issue.
Yes, the trajectory_duration is the parameter that I was referring to.
We are currently working on some improvements for STOMP which should address the oscillatory behavior you are seeing, I'll let you know once we get those changes into the main repo.
Thanks

Jorge

Teresa Seco

unread,
Mar 23, 2016, 7:22:47 AM3/23/16
to swri-ros-pkg-dev
It would be great!

Thank you Jorge

Jorge Nicho

unread,
Mar 30, 2016, 3:05:39 PM3/30/16
to swri-ros-pkg-dev
Teresa,
Sorry for the late reply but I just wanted to let you know that the improvements to STOMP have been merged, briefly the significant improvements are:
 - Returns smooth trajectories, no more wobbly motions
 - Uses the finite difference matrix in order to calculate velocities and accelerations

Let me know how it goes if you get a chance to test it.  Thanks

Jorge

On Friday, March 4, 2016 at 4:32:14 AM UTC-6, Teresa Seco wrote:
Message has been deleted

Teresa Seco

unread,
Mar 31, 2016, 5:42:54 AM3/31/16
to swri-ros-pkg-dev
Hello Jorge,

Thank you very much.

I have just tested it with our ur10 robot (with Gazebo once the problem with the start point was solved).
You can see the result of the test in this video:
https://drive.google.com/file/d/0B1OwbapT2nrsbmo0M1VQVUhqcEE/view?usp=sharing

To sumarize:

- When the movement of the robotic arm implies quite a large displacement (x position from 0.500 to -0.500), the planner trajectory is ok.
- When the movement is a short one, the planner trajectory is a littel weird
- When the movement  of the robot is along z axis (from 0.500 to 0.700 or 0.700), the planner returns a trajectory, the robot makes a short movement and then it finishes in the same start postion. The messages returned by moveit are as the robot had reached the target postion, with no failure (/follow_joint_trajectory/result SUCCEEDED).

The stomp_planning.yaml that we use is:


# Optimization Task
num_feature_basis_functions: 1
trajectory_duration: 8.0
num_time_steps: 20 #40 #20

publish_trajectory_markers: False
publish_best_trajectory_marker: True


# STOMP
max_iterations: 10
max_iterations_after_collision_free: 2
max_rollouts: 10 #100
min_rollouts: 10
num_rollouts_per_iteration: 10 #20


# STOMP (optional)
use_noise_adaptation: true
write_to_file: false
use_openmp: false

noise_coefficients:
 - group_name: manipulator
   stddev: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05]
   min_stddev: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01]
   decay:  [0.999, 0.999, 0.999, 0.999, 0.999, 0.999]



features:
- class: stomp_moveit_interface/ObstacleAvoidanceFeature
  collision_clearance: 0.02

With these parameters the planning time is better than with the previous one (max_rollouts = 100 and num_rollouts_per_iteration = 20).
Any suggestions?

On the other hand, we checked this planner with and object in the scene using a camera and octomap, and although the returned trajectory is the best of the other planners checked (ompl), the planning time is very hignt (around 60 seconds). As you mentioned in previous post, I suppose that you are working around stomp and octomap. It would be very aprerciated that you let me know any improvements you make in this sense.

Thank you very much for your help, it is being very vaulable for us to go on with our applcation.

Teresa

Jorge Nicho

unread,
Mar 31, 2016, 11:36:06 AM3/31/16
to swri-ros-pkg-dev
Teresa,
Have you checked if the IK returns a valid solution?  In your video, you change the z value by 0.30 meters but the orientation remains the same, so the resulting tool position may not be reachable by the robot.  

Generally, num_rollouts_per_iteration=10 tends to work well if the planning problem isn't too difficult (obstacles are not in the way).  You'll have to play around with those values a bit in order to find a configuration that is most appropriate as there isn't a general configuration
that works for most cases.

STOMP uses FCL to check for collisions and calculate distances but unfortunately it doesn't handle octomaps very efficiently; for now I would suggest making your octomaps very coarse by increasing the voxel leaf size in order to improve planning time.

Hvass, Paul B.

unread,
Mar 31, 2016, 12:06:44 PM3/31/16
to swri-ros...@googlegroups.com

+1

--

Teresa Seco

unread,
Apr 1, 2016, 3:20:57 AM4/1/16
to swri-ros-pkg-dev
Jorge,

Yes, I have checked the solution and it is valid. I usually reproduce the movements that have been tested with other planners. Moreover moveit does not return any error, so I do not know what is the reason for this behaviour....

Jorge Nicho

unread,
Apr 1, 2016, 2:21:50 PM4/1/16
to swri-ros-pkg-dev
Teresa,
Out of curiosity, how are you checking that the IK solution is valid?
Thanks


On Friday, March 4, 2016 at 4:32:14 AM UTC-6, Teresa Seco wrote:

Teresa Seco

unread,
Apr 4, 2016, 3:57:48 AM4/4/16
to swri-ros-pkg-dev
Jorge,

I tried the same movements with ompl planners and SetPoseTarget() function and the robot reached the target pose, for these reason I supposed that the pose was reachable.

I have just checked with ompl planners but using joint space goal and then IK to objtain the value of each joint and you are right, the same behaviour as STOMP is observed!

The code used for IK (for a particular geometry_msgs::Pose targetPose) is:

       
        robot_state::RobotStatePtr robotState;
        robotState
= group_->getCurrentState();
        group_
->setStartState(*robotState);

       
       
//joint space goal

        robot_model_loader
::RobotModelLoader robot_model_loader("robot_description");
        robot_model
::RobotModelPtr robot_model_ptr = robot_model_loader.getModel();
        robot_state
::RobotStatePtr robot_state_ptr(group_->getCurrentState()); //copy state

     
       
const robot_state::JointModelGroup* joint_model_group = robot_model_ptr->getJointModelGroup(group_->getName());
       

       
bool found_ik = robot_state_ptr->setFromIK(joint_model_group, targetPose, 0, 0);

       
if (found_ik)
            ROS_INFO
("Ik found");
       
else
            ROS_INFO
("Ik not found");


        std
::vector<double> jointValues;
        robot_state_ptr
->copyJointGroupPositions("manipulator", jointValues);

        ROS_INFO
("joint_values: %f %f %f %f %f %f",jointValues[0],jointValues[1],jointValues[2],jointValues[3],jointValues[4],jointValues[5]);
        group_
->setJointValueTarget(jointValues);

The problem is then that the IK does not find a valid solution, but, is there other way to obtain the joints goal from a geometry_msgs::Pose object?

Sorry for my misunderstanding

Jorge Nicho

unread,
Apr 4, 2016, 2:48:35 PM4/4/16
to swri-ros...@googlegroups.com
Teresa,
Judging from the video you provided, it looks like some of the tool poses were not achievable , meaning that there were no valid joint configurations that would allow the robot to set the tool in that cartesian pose.  You could try changing the orientation of the tool as long as the position is reachable. If you are using the IK KDL numerical solver you could try increasing the number of attempts or timeout values in the "kinematics.yaml" file, of course these changes will not help if the pose is unreachable.  I noticed that your gui allows changing the orientation by tweaking the quaternion values x, y, z and w.  Quaternions aren't very intuitive and thus is difficult to visualize what the final pose will be; I'd recommend using Euler angles instead. Thanks

Jorge

--
You received this message because you are subscribed to a topic in the Google Groups "swri-ros-pkg-dev" group.
To unsubscribe from this topic, visit https://groups.google.com/d/topic/swri-ros-pkg-dev/sNvFmkQsMtg/unsubscribe.
To unsubscribe from this group and all its topics, send an email to swri-ros-pkg-d...@googlegroups.com.

Teresa Seco

unread,
Apr 5, 2016, 6:20:27 AM4/5/16
to swri-ros-pkg-dev
Hi Jorge,

Playing around with the joints limits in the urdf model, we managed to reach some positions that were not before. We fixed these limits with the real robot to avoid certains movements of the robot. For this reason, the IK solver sometimes is not capable to return a solution. I am a little confuse about why using carthesian paths the position is reachable but with joint space state is not reachable...

Only to clirify about the orientation values used in rqt gui. The orientation x,y,z values are in Euler angles and the in my code I convert them to quaternion. It was a trick to work with Euler angles using a geometry_msgs::PosStamped object.

Thank you Jorge

jrgn...@gmail.com

unread,
Apr 6, 2016, 3:28:16 PM4/6/16
to swri-ros-pkg-dev
Teresa,

How are you comparing the cartesian planning vs joint space planning?  In theory, if the tool pose is achievable, meaning there exist a joint configuration that allows the robot to place the tool in the desired cartesian pose, then the joint space planner should be able to plan for that.  

Thanks

Teresa Seco

unread,
Apr 7, 2016, 3:23:01 AM4/7/16
to swri-ros-pkg-dev, jrgn...@gmail.com
Hi Jorge,


The target pose is always a geometry_msgs::Pose targetPose. Then, so to use a catesian pose the function used for that is setPoseTarget:

   group_->setPoseTarget(targetPose);


For joint space goal, first of all I need to obtain the joints values corresponding to these target pose, so the implemented code is:


        //joint space goal
        robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
        robot_model::RobotModelPtr robot_model_ptr = robot_model_loader.getModel();
        robot_state::RobotStatePtr robot_state_ptr(group_->getCurrentState()); //copy state
      
        const robot_state::JointModelGroup* joint_model_group = robot_model_ptr->getJointModelGroup(group_->getName());
      
        bool found_ik = robot_state_ptr->setFromIK(joint_model_group, targetPose, 10, 0);


        if (found_ik)
            ROS_INFO("Ik found");
        else
            ROS_INFO("Ik not found");

        std::vector<double> jointValues;
        robot_state_ptr->copyJointGroupPositions("manipulator", jointValues);
        group_->setJointValueTarget(jointValues);

The following code is common for both to plan and execute:


    moveit::planning_interface::MoveGroup::Plan plan;
    bool success = group_->plan(plan);
    ROS_INFO("Visualizing plan  (pose goal) %s",success?"":"FAILED");
    //sleep(5.0);    // para ver el plan en el RViz

    if(isExecutePlan())
        group_->asyncExecute(plan);

In the next videos you can see the behaviour in both cases, with the same sequence of target poses. Using carthesian poses, the planner found the solution and execute it (from z = 0.500 to 0.800). Using joint space goals, the inverse kinematics (setFromIK) does not find a solution for that target. Perhaps I am doing something wrong.

https://drive.google.com/file/d/0B1OwbapT2nrsV0haZm5LLUg3THM/view?usp=sharing
https://drive.google.com/file/d/0B1OwbapT2nrsSW5PQXF2LTFmdTA/view?usp=sharing

Thank you very much.

Teresa

Hvass, Paul B.

unread,
Apr 11, 2016, 11:12:16 AM4/11/16
to swri-ros...@googlegroups.com

Hi Teresa,

 

You may want to try out the TRAC_IK numerical solver: http://www.ros.org/news/2015/11/introducing-a-better-inverse-kinematics-package.html

 

The probability of finding a valid solution is reported to be much improved with the TRAC_IK solver…

 

Paul Hvass

Program Manager

ROS-Industrial Consortium Americas

SwRI Robotics and Automation Engineering

210.522.5823 - W

210.896.0270 - M

Paul....@swri.org

ROSindustrial.org

--

You received this message because you are subscribed to the Google Groups "swri-ros-pkg-dev" group.

To unsubscribe from this group and stop receiving emails from it, send an email to swri-ros-pkg-d...@googlegroups.com.

juand...@gmail.com

unread,
Apr 11, 2016, 12:31:26 PM4/11/16
to swri-ros-pkg-dev
Hola Teresa y Jorge,

I have been following your posts, I am also using the UR10 arm with MoveIt! and after noticing that the OMPL planners were not that good, I decided to try STOMP. It finds better paths, but takes too long, I will use your yaml to test it.

I wanted to ask both of you if it is possible to load OPML and STOMP at the same time, so that I can choose which one to use for each case. For example, Im using RRTstarkConfigDefault and if it fails, I would use STOMP, just setting it in my code as I did with group.setPlannerId("RRTstarkConfigDefault"); . Would that be possible? Can we have two planning pipelines? I have tried, but just one (ompl) loads, but those launch files are confusing for me.

Regarding your IK problem, it also happens to me. Some configurations require more time, and may fail. My solution was to calculate the IK 10 times (each with timeout of 0.2 and 20 attempts), after that I choose the best solution. That also helps for some configurations that are close to the current one, but for some reason another configuration is found first, forcing the planner to create a longer path. I tried to install fast_IK, but the documentation was not enough to proceed in ubuntu 14. Let me know if you test TRAC_IK.

Regards,

Juan Daniel Arango Castellanos
University of Bremen

Jorge Nicho

unread,
Apr 12, 2016, 2:18:34 PM4/12/16
to swri-ros-pkg-dev, jrgn...@gmail.com
Teresa,
I assumed that you are using the KDL's IK numerical solver; which is the default solver used by moveit.  This solver takes an initial guess and it attempts to converge to a valid joint pose from the guess.  Moveit passes the robot state current joint pose as the initial guess so if this isn't close enough to the solution then it'll have a more difficult time finding a valid IK solution.  I'd recommend setting the robot state joint values to the current joint pose   before calling the "setFromIK" method.  You could also increase the values on the kinematics.yaml file located in the config directory of the moveit_config package for your robot.
Furthermore, you should probably return or exit your function call when a solution isn't found since the robot state may end up in an undetermined state; executing this on real hardware could be rather risky.  
Thank you

Jorge

shantan...@gmail.com

unread,
Mar 28, 2017, 2:52:26 PM3/28/17
to swri-ros-pkg-dev
Hi Juan,

I see that you were able to use STOMP planner with UR10 in Moveit!. I am trying to use moveit industrial (https://www.google.com/search?client=ubuntu&channel=fs&q=moveit+industrial&ie=utf-8&oe=utf-8) with the UR5 package (https://github.com/ros-industrial/universal_robot). I have created files like stomp_planning_pipeline.launch.xml, and made changes in the move_group.launch file. I also used the stomp_config.yaml file from this forum but it is not working. Can you please tell me what steps you followed to make UR10 work with STOMP in moveit! ?

Thank you !

Regards
Shantanu Thakar
University of Southern California

Jorge Nicho

unread,
Mar 30, 2017, 11:15:07 AM3/30/17
to swri-ros-pkg-dev
Hello Shantanu,
Can you elaborate more on the issues you are having setting up stomp with your robot?  You can try following the instructions here for setting up stomp.  Thanks

Juan Daniel Arango

unread,
Mar 31, 2017, 4:09:15 AM3/31/17
to swri-ros...@googlegroups.com
Hello Shantanu, 

I used the example included with STOMP, tracked the files that were needed for that example to run and after some time changing and creating files, it worked!
I can not tell you anymore exactly how I did it, since Im not working there anymore. 

Mit freundlichen Grüßen,  

Juan Daniel Arango C.
M.Sc. Information and Automation Engineering


To unsubscribe from this group and all its topics, send an email to swri-ros-pkg-dev+unsubscribe@googlegroups.com.

Shantanu Thakar

unread,
Apr 3, 2017, 2:22:59 PM4/3/17
to swri-ros...@googlegroups.com
Hi Jorge,

I was able to use STOMP with UR5 and it works perfectly. I was using the wrong stomp_config.yaml file.

Thanks !

 
Shantanu Thakar
PhD Student
Realization of Robotics Systems Lab
AME, Viterbi School of Engineering
University of Southern California
Los Angeles, CA 90089

Jorge Nicho

unread,
Apr 3, 2017, 2:27:57 PM4/3/17
to swri-ros-pkg-dev
Shantanu,
I'm glad you got that resolved.  Feel free to create new issues or PR's in the github page here.
Thanks

Jorge Nicho | Research Engineer
Southwest Research Institute


Intelligent Systems Division

Manufacturing Technologies Department
Adaptive Technologies Section 
To unsubscribe from this group and all its topics, send an email to swri-ros-pkg-d...@googlegroups.com.

For more options, visit https://groups.google.com/d/optout.

--
You received this message because you are subscribed to a topic in the Google Groups "swri-ros-pkg-dev" group.
To unsubscribe from this topic, visit https://groups.google.com/d/topic/swri-ros-pkg-dev/sNvFmkQsMtg/unsubscribe.

mic...@xihelm.com

unread,
Dec 25, 2017, 10:02:16 AM12/25/17
to swri-ros-pkg-dev
Hello Shantanu, 

I am using the UR5 with STOMP as well, but I am not sure about what values to use in the stomp_planning.yaml file. Could you point me to some appropriate ones? Thanks in advance, 

Michael
To unsubscribe from this group and all its topics, send an email to swri-ros-pkg-d...@googlegroups.com.

For more options, visit https://groups.google.com/d/optout.

--
You received this message because you are subscribed to a topic in the Google Groups "swri-ros-pkg-dev" group.
To unsubscribe from this topic, visit https://groups.google.com/d/topic/swri-ros-pkg-dev/sNvFmkQsMtg/unsubscribe.
Reply all
Reply to author
Forward
0 new messages