Orientation constraint problem

[David Brodeur]

Hi,

I am controling a Fanuc Lrmate i200c robot (6 joints) with moveit. I wish it could scan the top of a table, looking for a QR code and follow it when one is found.

When I generate the plan and then call execute(plan), the robot often moves link_4 to link_6 even if it is not necessary since the orientation of the end-effector should always stay the same (I need to keep the camera looking downward). So I added an orientation constraint without success because the plan() method cannot find a solution :

[ WARN] [1465936140.844850843]: ParallelPlan::solve(): Unable to find solution by any of the threads in 0.002094 seconds
[ERROR] [1465936140.844991643]: Can only compute FK for IKTYPE_TRANSFORM_6D!
[ERROR] [1465936140.845120431]: Can only compute FK for IKTYPE_TRANSFORM_6D!
[ERROR] [1465936140.845242583]: Can only compute FK for IKTYPE_TRANSFORM_6D!
[ERROR] [1465936140.845362151]: Can only compute FK for IKTYPE_TRANSFORM_6D!

Similar problem if I limit joint #4 position in the joint_limits.yaml file to +/- 1.57 rad. What I found to work best was to increase the number of planning attempts. However, this is not working all times. So I keep checking the speed values for joint #4 in the planned trajectory to see if it is zero. If not I compute the plan again.

I would really prefer to use constraints as specified in the tutorial but I don't see what to do. Is this related to the planner ID (RRTConnectConfigDefault)? Here is a part of my code with the constraint:

bool ActuatorFanuc::plan() {

    moveit_msgs::OrientationConstraint ocm;
    moveit_msgs::Constraints test_constraints;

    ocm.link_name = "link_4";
    ocm.header.frame_id = group_->getPoseReferenceFrame();
    ocm.orientation.x = 0.0;
    ocm.orientation.y = 0.0;
    ocm.orientation.z = 0.0;
    ocm.orientation.w = 1.0;
    ocm.absolute_x_axis_tolerance = 0.1;
    ocm.absolute_y_axis_tolerance = 0.1;
    ocm.absolute_z_axis_tolerance = 0.1;
    ocm.weight = 1.0;

    test_constraints.orientation_constraints.push_back(ocm);
    group_->setPathConstraints(test_constraints);

    stopped_ = false;

    group_->clearPoseTargets();
    group_->setPoseTarget(command_pose_);

    if (group_->plan(plan_)) {

        ROS_INFO("Fanuc robot trajectory planned.");
        return true;
    }

    else {

        ROS_WARN("Fanuc robot could not plan trajectory.");
        return false;
    }
}

[G.A. vd. Hoorn - 3ME]

On 28-6-2016 15:26, David Brodeur wrote:
>
> Hi,
>
> I am controling a Fanuc Lrmate i200c robot (6 joints) with moveit. I wish
> it could scan the top of a table, looking for a QR code and follow it when
> one is found.
>
> When I generate the plan and then call execute(plan), the robot often moves
> link_4 to link_6 even if it is not necessary since the orientation of the
> end-effector should always stay the same (I need to keep the camera looking
> downward). So I added an orientation constraint without success because the
> plan() method cannot find a solution :
>
> [ WARN] [1465936140.844850843]: ParallelPlan::solve(): Unable to find
> solution by any of the threads in 0.002094 seconds
> [ERROR] [1465936140.844991643]: Can only compute FK for IKTYPE_TRANSFORM_6D!
> [ERROR] [1465936140.845120431]: Can only compute FK for IKTYPE_TRANSFORM_6D!
> [ERROR] [1465936140.845242583]: Can only compute FK for IKTYPE_TRANSFORM_6D!
> [ERROR] [1465936140.845362151]: Can only compute FK for IKTYPE_TRANSFORM_6D!

If you are using the fanuc_lrmate200ic_support and
fanuc_lrmate200ic_moveit_plugins packages, then this could be an ik issue.

Try the recently released trac_ik plugin (configure it through the
moveit setup assistant), and set it to solve for 'distance' (not 'speed').

[David Brodeur]

Ok thanks for the answer.  I have installed trac_ik_kinematic_plugin, trac_ik_lib and trac_ik_examples. 

I am not really familiar with moveit setup assistant, but I followed the tutorial and edited the fanuc_lrmate200ic_moveit_config package by setting the kinematic solver to trac_ik. Then, I opened the kinematic.yaml file and added solve_type: Distance.

I guess that is not what I should have done. This gives me a new error : 

Parameter 'moveit_controller_manager' not specified. This is needed to identify the plugin to use for interacting with controllers. 

[...]

IK cannot be performed for 'link_4'. The solver can report IK solutions for link 'tool0'.

I'll try to search/read more on kinematic solvers. 

Thanks again.

[G.A. vd. Hoorn - 3ME]

On 28-6-2016 16:52, David Brodeur wrote:
>
> Ok thanks for the answer. I have installed trac_ik_kinematic_plugin,
> trac_ik_lib and trac_ik_examples.
>
> I am not really familiar with moveit setup assistant, but I followed the
> tutorial and edited the fanuc_lrmate200ic_moveit_config package by setting
> the kinematic solver to trac_ik. Then, I opened the kinematic.yaml file and
> added solve_type: Distance.
>
> I guess that is not what I should have done. This gives me a new error :
>
> Parameter 'moveit_controller_manager' not specified. This is needed to
> identify the plugin to use for interacting with controllers.
>
> [...]
>
> IK cannot be performed for 'link_4'. The solver can report IK solutions for
> link 'tool0'.
>
> I'll try to search/read more on kinematic solvers.

You probable let the setup assistant regenerate all files. It probably
overwrote the 'fanuc_lrmate200ic_moveit_controller_manager.launch.xml'
file, which sets up the controllers for moveit.

The only file it needed to generate was kinematics.yaml.

It's probably easiest to revert all your changes except those to
kinematics.yaml. That should sort things out.


Gijs

[David Brodeur]

I have reinstalled all the fanuc ros packages from scratch and just modifed the few lines in kinematics.yaml : 

manipulator:

  kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin

  kinematics_solver_timeout: 0.005

  solve_type: Distance

There was no orientation constraint in my code. The error received is:

Unable to construct goal representation

Fail: ABORTED: Catastrophic failure. 

Trying to read about it in other posts : 

https://groups.google.com/forum/#!topic/moveit-users/hux5NJ1LEGs

https://groups.google.com/forum/#!topic/moveit-users/N2VsBEf0c7k

If I find any fix, I'll post it here.

Thanks again!

[G.A. vd. Hoorn - 3ME]

Just ran a quick test myself (using demo.launch, your kinematics.yaml
and the RViz MoveIt plugin), and everything seems to work correctly here.

Not sure why you are receiving that error.

> If I find any fix, I'll post it here.

That would be very nice.


Gijs

[David Brodeur]

Ok thanks,

I effectively see no error when using demo.launch + kinematics.yaml (with TRAC_IK) + RViz MoveIt plugin.

The error appears using move_group, in the Plan() method. When I use the default kinematics.yaml, it works fine unless the fact I am not yet able to add orientation constraints.

I didn't mean to pretend there is a bug to fix in the moveit/fanuc software. Just probably something in it that I don't understand yet and that would fix the issue in my own code.

Thanks again,

David

[David Brodeur]

Using TRAC-IK definitively improved the trajectory path as I no longer observe singularities issues. It seems to work well without the need to add a constraint.

However, I get a new error :

[ INFO] [1467901276.282514693]: Using solve type Speed
[ INFO] [1467901276.283310087]: Planner configuration 'manipulator[RRTConnectkConfigDefault]' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1467901276.284918586]: Allocating specialized state sampler for state space
[ INFO] [1467901276.285009805]: manipulator[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1467901276.285221097]: Allocating specialized state sampler for state space
[ INFO] [1467901276.285298329]: manipulator[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1467901276.285365676]: Allocating specialized state sampler for state space
[ INFO] [1467901276.285444389]: Allocating specialized state sampler for state space
[ INFO] [1467901276.285506433]: manipulator[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1467901276.285573661]: manipulator[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure

***** Internal Program Error - assertion (satisfiesBounds(state1) && satisfiesBounds(state2)) failed in virtual double ompl::base::SO3StateSpace::distance(const ompl::base::State*, const ompl::base::State*) const:
/tmp/binarydeb/ros-indigo-ompl-1.0.0003094/src/ompl/base/spaces/src/SO3StateSpace.cpp(284): The states passed to SO3StateSpace::distance are not within bounds. Call SO3StateSpace::enforceBounds() in, e.g., ompl::control::ODESolver::PostPropagationEvent, ompl::control::StatePropagator, or ompl::base::StateValidityChecker
[move_group-4] process has died [pid 5962, exit code -6, cmd /opt/ros/indigo/lib/moveit_ros_move_group/move_group --debug __name:=move_group __log:=/home/david/.ros/log/023c7f0e-444e-11e6-b682-00224d829992/move_group-4.log].
log file: /home/david/.ros/log/023c7f0e-444e-11e6-b682-00224d829992/move_group-4*.log
^C[actuator_fanuc-7] killing on exit


What I have read so far about this error suggest to change default planner ID and use RRTConnect. However, that was already what I was doing. The only thing I see, is that for some reason, the solver_type is set back to Speed even if I did write "Distance" in the kinematic.yaml file :
manipulator:
  kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
  kinematics_solver_timeout: 0.1
  solve_type: "Distance"

  ### DEFAULT ###
  #kinematics_solver: fanuc_lrmate200ic_manipulator_kinematics/IKFastKinematicsPlugin
  #kinematics_solver_search_resolution: 0.005
  #kinematics_solver_timeout: 0.005
  #kinematics_solver_attempts: 3

David

[Patrick Beeson]

On Thursday, July 7, 2016 at 9:50:14 AM UTC-5, David Brodeur wrote:

The only thing I see, is that for some reason, the solver_type is set back to Speed even if I did write "Distance" in the kinematic.yaml file :
manipulator:
  kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
  kinematics_solver_timeout: 0.1
  solve_type: "Distance"

  ### DEFAULT ###
  #kinematics_solver: fanuc_lrmate200ic_manipulator_kinematics/IKFastKinematicsPlugin
  #kinematics_solver_search_resolution: 0.005
  #kinematics_solver_timeout: 0.005
  #kinematics_solver_attempts: 3

David

You need to load the kinematics parameters for your specific ROS node that loads the MoveIt! planner.  Please see this description of this problem.  https://bitbucket.org/traclabs/trac_ik/issues/25/unable-to-set-solver_type-in-launch-file

[David Brodeur]

Ok, thanks. The description is really clear and now I load the right parameters. I still get the same error when adding a constraint, but now I am sure it is not caused by the kinematics parameters.


Recall of the error :

[ INFO] [1467917062.588235362]: Looking in private handle: /move_group for param name: manipulator/position_only_ik
[ INFO] [1467917062.590039606]: Looking in private handle: /move_group for param name: manipulator/solve_type
[ INFO] [1467917062.591745304]: Using solve type Distance
[ INFO] [1467917062.592542537]: Planner configuration 'manipulator[RRTConnectkConfigDefault]' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1467917062.595101464]: Allocating specialized state sampler for state space
[ INFO] [1467917062.595219453]: manipulator[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1467917062.595316832]: Allocating specialized state sampler for state space
[ INFO] [1467917062.595417442]: manipulator[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1467917062.595524162]: Allocating specialized state sampler for state space
[ INFO] [1467917062.595601353]: manipulator[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1467917062.595663131]: Allocating specialized state sampler for state space
[ INFO] [1467917062.595755133]: manipulator[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ERROR] [1467917067.596270770]: manipulator[RRTConnectkConfigDefault]: Unable to sample any valid states for goal tree
[ERROR] [1467917067.596329211]: manipulator[RRTConnectkConfigDefault]: Unable to sample any valid states for goal tree
[ INFO] [1467917067.596367951]: manipulator[RRTConnectkConfigDefault]: Created 1 states (1 start + 0 goal)
[ERROR] [1467917067.596421394]: manipulator[RRTConnectkConfigDefault]: Unable to sample any valid states for goal tree
[ INFO] [1467917067.596456263]: manipulator[RRTConnectkConfigDefault]: Created 1 states (1 start + 0 goal)
[ INFO] [1467917067.596527212]: manipulator[RRTConnectkConfigDefault]: Created 1 states (1 start + 0 goal)
[ERROR] [1467917067.599260610]: manipulator[RRTConnectkConfigDefault]: Unable to sample any valid states for goal tree
[ INFO] [1467917067.599307136]: manipulator[RRTConnectkConfigDefault]: Created 1 states (1 start + 0 goal)
[ WARN] [1467917067.599415247]: ParallelPlan::solve(): Unable to find solution by any of the threads in 5.006638 seconds
[ INFO] [1467917067.819121418]: Unable to solve the planning problem
[ WARN] [1467917067.819615223]: Fail: ABORTED: No motion plan found. No execution attempted.
[ WARN] [1467917067.819679750]: Fanuc robot could not plan trajectory.
[ INFO] [1467917068.820442146]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1467917068.830838939]: IK Using joint link_1 -2.9671 2.9671
[ INFO] [1467917068.830908442]: IK Using joint link_2 -1.0472 2.4435
[ INFO] [1467917068.830945331]: IK Using joint link_3 -2.4784 4.0143
[ INFO] [1467917068.830984398]: IK Using joint link_4 -3.3161 3.3161
[ INFO] [1467917068.831020423]: IK Using joint link_5 -2.0944 2.0944
[ INFO] [1467917068.831058073]: IK Using joint link_6 -6.2832 6.2832
[ INFO] [1467917068.831091139]: Looking in private handle: /move_group for param name: manipulator/position_only_ik
[ INFO] [1467917068.833075044]: Looking in private handle: /move_group for param name: manipulator/solve_type
[ INFO] [1467917068.835146763]: Using solve type Distance
[ INFO] [1467917068.836324507]: Planner configuration 'manipulator[RRTConnectkConfigDefault]' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1467917068.838244395]: Allocating specialized state sampler for state space
[ INFO] [1467917068.838316803]: manipulator[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1467917068.838551890]: Allocating specialized state sampler for state space
[ INFO] [1467917068.838640797]: manipulator[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1467917068.838816130]: Allocating specialized state sampler for state space
[ INFO] [1467917068.838917342]: manipulator[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1467917068.839017655]: Allocating specialized state sampler for state space
[ INFO] [1467917068.839109128]: manipulator[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure

***** Internal Program Error - assertion (satisfiesBounds(state1) && satisfiesBounds(state2)) failed in virtual double ompl::base::SO3StateSpace::distance(const ompl::base::State*, const ompl::base::State*) const:
/tmp/binarydeb/ros-indigo-ompl-1.0.0003094/src/ompl/base/spaces/src/SO3StateSpace.cpp(284): The states passed to SO3StateSpace::distance are not within bounds. Call SO3StateSpace::enforceBounds() in, e.g., ompl::control::ODESolver::PostPropagationEvent, ompl::control::StatePropagator, or ompl::base::StateValidityChecker

[move_group-4] process has died [pid 7437, exit code -6, cmd /opt/ros/indigo/lib/moveit_ros_move_group/move_group --debug __name:=move_group __log:=/home/david/.ros/log/c100d916-4472-11e6-99a3-00224d829992/move_group-4.log].
log file: /home/david/.ros/log/c100d916-4472-11e6-99a3-00224d829992/move_group-4*.log

[David Brodeur]

I will keep investigating toward RRTConnect and its parameters. Maybe I need to change something there too.

[David Brodeur]

It looks like part of my error is gone when using Speed instead of Distance as solve_type. So the error is reduced to that :

[Mark Moll]

This is very strange. As far as I know, MoveIt! doesn’t expose OMPL's kinodynamic planners (anything in the ompl::control namespace). Somehow you are calling the ODE solver which is used for numerical integration of your dynamics equations.   If this is actually what is supposed to happen, then you need to call enforceBounds at the end of ompl::control::ODESolver::PostPropagationEvent, ompl::control::StatePropagator, or ompl::base::StateValidityChecker, just like the error message says.

Mark

[David Brodeur]

Thanks for your answer. That is what I also read. However, I don't understand where I am actually calling that ODE solver using the move_group interface...

Attached, you'll find my code and launch file and modified config files in fanuc_lrmate200ic_moveit_config are shown below. The problem comes when I had a constraint (see createOrientationConstraint() in actuator_fanuc.cpp) in the init() method (actuator_fanuc.cpp). The init method executes 2 trajectories. The first one has no constraint and works perfectly. The second trajectory adds a constraint to keep the orientation of the end effector at the end of the first trajectory. This assures that the start state is satisfies the contraint and that it is possible to plan the second trajectory. Am I right?

kinematics.yaml :

manipulator:
  kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
  kinematics_solver_timeout: 0.1

  solve_type: "Speed"


joint_limits.yaml :

# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed
# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
joint_limits:
  joint_1:
    has_position_limits: false
    min_position: -1.5708
    max_position: 1.5708
    has_velocity_limits: true
    max_velocity: 6.1087
    has_acceleration_limits: true
    max_acceleration: 1.22174
  joint_2:
    has_velocity_limits: true
    max_velocity: 6.1087
    has_acceleration_limits: true
    max_acceleration: 1.22174
  joint_3:
    has_velocity_limits: true
    max_velocity: 6.9813
    has_acceleration_limits: true
    max_acceleration: 1.39626
  joint_4:
    has_velocity_limits: true
    max_velocity: 7.854
    has_acceleration_limits: true
    max_acceleration: 1.5708
  joint_5:
    has_velocity_limits: true
    max_velocity: 7.854
    has_acceleration_limits: true
    max_acceleration: 1.5708
  joint_6:
    has_velocity_limits: true
    max_velocity: 12.5664
    has_acceleration_limits: true
    max_acceleration: 2.51328

[G.A. vd. Hoorn - 3ME]

On 07/08/2016 03:00 PM, David Brodeur wrote:
> joint_limits.yaml :
[..]
> joint_limits:
> joint_1:
> *has_position_limits: false*
> *min_position: -1.5708*
> *max_position: 1.5708*
[..]

bit of an off-topic question perhaps, but why would you want to disable
the position_limits here?


Gijs

[David Brodeur]

In fact, there was no position limits in the default joint_limits.yaml file. I added those 3 lines myself while doing some tests, so I put false instead of puttong those lines in comments. Will try to comment them.

[David Brodeur]

Same problem as before if I use the default joint_limits.yaml.

[G.A. vd. Hoorn - 3ME]

On 07/08/2016 03:15 PM, David Brodeur wrote:
>
> In fact, there was no position limits in the default joint_limits.yaml
> file. I added those 3 lines myself while doing some tests, so I put false
> instead of puttong those lines in comments. Will try to comment them.

O right, yes. I got confused with the velocity and accel limits.

It'll just default to the limits in the urdf.

sorry for the noise.


Gijs

[David Brodeur]

No problem. I'll keep up looking at how OMPL works through MoveIt!. As you will see in my code, my ultimate goal is to have my ROS node "actuator_fanuc" wrap all the MoveIt! / move_group code so that commands can be sent from other nodes.

[David Brodeur]

I hate to say that, but I think that I found the solution : sudo apt-get install ros-indigo-ompl*

It seems that the ROS package was not up to date with the version I had. Can't say if it was installed with the installation of the full desktop ROS indigo version or later, along with other ROS-Industrial package. Maybe an older version replaced the good one after reinstalling other ROS Moveit packages. Yesterday, I realized that calling sudo apt-get install ros-indigo-moveit* to add missing packages had removed some fanuc packages. It might be similar for OMPL.

Anyway, now it seems to work so I am going to do more exhaustive testing. Meanwhile I learned a lot of details about Moveit, TRAC-IK and OMPL from all of you.

Thanks again!

[David Brodeur]

Might be related to that post about new OMPL release : https://groups.google.com/forum/#!topic/moveit-users/sEqP6iXOrVc

As I was new to MoveIt! and didn't know anything yet about OMPL, I did not pay attention to it earlier.

[G.A. vd. Hoorn - 3ME]

do you have a source install of MoveIt?

[David Brodeur]

No I installed the released version using sudo apt-get install ros-indigo-moveit*

Same thing for Fanuc ros packages, but I have also tried in the last weeks to install Fanuc code from source.