pose of the trajectory not quite right!?

[blackhorsewu]

I have copied the tutorial1 but changed it for UR5 with a welding torch as the end effector. I called the tip of the torch as tcp. The tutorial compiled and executed. Except it does not move at the position that I understand as it should. The manipulator, as I understand it should move along parallel to the Y axis at a fixed X displacement from the origin of "world". However, it moves parallel to the X axis at a fixed -ve Y displacement. Is there any thing that I could have done wrong?
I have done this tutorial before it was changed from Affine3d to Isometry3d and it did not behave this strangely.
Your help would be very much appreciated.

[JeroenDM]

Did you change "base_link" to "world" frame in the tutorial code here?

  // Name of frame in which you are expressing poses. Typically "world_frame" or "base_link".
  const std::string world_frame = "base_link";

Maybe there is a rotation between world and base_link, and as the tutorial uses base_link by default this could cause the path to be rotated.

You could also check if you have the latest version of the tutorial and the makePath() function does create a path parallel to Y on this line.

// set the translation (we're moving along a line in Y)
pose.translation() = Eigen::Vector3d(0, i * step_size, 0);

These are just guesses of course :) If you could show the code you're using it's easier help you.

[blackhorsewu]

@JeroenDM Thank you very much for your reply.

Yes, I have indeed changed the world_frame from base_link to world.

However, I am now even more confused after some investigation. I found that when I said it work for me before, I called the tip link as tool0 and actually change the urdf file ur5.urdf.xacro and the axis of the tip link are parallel to those of wrist_3_link. Now, I have changed my tip link and call it tcp and it is attached to the wrist_3_link and the axis at the tip (of the welding torch) has the z axis along the centre of the torch. With this Descartes almost failed to plan any path for me, I have tried different paths all over the places. I am convenced that all the problems are related with the joints and links and the orientations of the axis.

Is there any principle that I should stick to for making sure they are consistant and conform to the conventions of the Descartes package.

Your help is very much appreciated.

[JeroenDM]

There are some general guidelines for industrial robot URDF files here.

Based on your explanation I suspect it could be an issue with the inverse kinematics solver. When you change links in the urdf file, you should rerun the MoveIt setup assistant and select the correct base link and tip link when setting up the inverse kinematics solver.

In that case, you could use the default KDL inverse kinematics solver, as done in the original UR 5 package here. The (faster) analytical solvers from ur_kinematics are only valid for a specific urdf file I think. You should also change the tutorial code, as the IKFast solver is assumed as the default:

  // All of the existing planners (as of Nov 2017) have been designed with the idea that you have "closed form"
  // kinematics. This means that the default solvers in MoveIt (KDL) will NOT WORK WELL. I encourage you to produce
  // an ikfast model for your robot (see MoveIt tutorial) or use the OPW kinematics package if you have a spherical
  // wrist industrial robot. See http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/ikfast/ikfast_tutorial.html 

  // This package assumes that the move group you are using is pointing to an IKFast kinematics plugin in its
  // kinematics.yaml file. By default, it assumes that the underlying kinematics are from 'base_link' to 'tool0'.
  // If you have renamed these, please set the 'ikfast_base_frame' and 'ikfast_tool_frame' parameter (not in the
  // private namespace) to the base and tool frame used to generate the IKFast model.
  descartes_core::RobotModelPtr model (new descartes_moveit::IkFastMoveitStateAdapter());

So (against the recommendation in the comments) you could change the last line of code into

  descartes_core::RobotModelPtr model (new descartes_moveit::MoveitStateAdapter());

and add the corresponding include header at the top of the tutorial script:

#include <descartes_moveit/moveit_state_adapter.h>

But, as mentioned in the comments, this will be much much slower. Another option is leaving the urdf file unchanged and set the correct the tool offset and rotation using the tool_pt argument of a CartTrajectoryPt in descartes. I've never used this so I'm not sure how it works, but there are some comments in the source code.

[JeroenDM]

Maybe as a general remark, there seem to be several issues regarding the inverse kinematics of the UR robots. I'm not sure what the current status or recommendation is... maybe @gavanderhoorn knows?

[blackhorsewu]

@JeroenDM Thank you very much for your help. I have changed from using the IkFastMoveitStateAdapter to MoveitStateAdapter, though as you said it is much slower, the much higher chance of path can be found. I have also changed from ur_kinematics to use kdl_kinematics. Now I really want to be able to fully control the orientation of the tool and not leaving one of the rotation free. I have tried not to use makeTolerancedCartesianPoint but juse makeCartesianPoint. However, it will be successful in planning a path some of the time and most of the time it will fail to plan a path even though the way points are exectly the same.
Is there anyway to fix the orientation completely?

[JeroenDM]

Is there anyway to fix the orientation completely?

I assume you want to fix both orientation and position.

You can use zeroTolerance for position and orientation constraints, as is done in this test. (It is often a good idea to look at the tests if you want to understand a package.) I think the code will look something like this:

using namespace descartes_trajectory;

Eigen::Isometry3d nominal_pose = ... // whatever nominal pose you need

Eigen::Vector3d t = a.translation();
Eigen::Matrix3d m = a.rotation();
Eigen::Vector3d rxyz = m.eulerAngles(0, 1, 2);

CartTrajectoryPt zero_tol_pose(
    TolerancedFrame(
        nominal_pose,
        ToleranceBase::zeroTolerance<PositionTolerance>(t(0), t(1),t(2)),
        ToleranceBase::zeroTolerance<OrientationTolerance>(rxyz(0), rxyz(1), rxyz(2))
    ),
    0, // pos_increment
    0  // orient_increment
);

(I did not test this code.) It is probably best to wrap it in a function to create the different trajectory points along the path.

[blackhorsewu]

Thanks very much. However, I am really lost with Eigen! where is the a come from? Is it the pose that I want and of type Eigen::Isometry3d? what does the rxyz mean? is it suppose to be quaternion but it is of Vector3d.
m.eulerAngles, what is m here?
Sorry for my ignorance. Please help or point me to some documentation explaining the use of Eigen.

[JeroenDM]

Eigen is a general-purpose C++ math library. It has good tutorials on the website: the basics and geometry module. (Although there seems to be an issue with the sidebar menu, in my browser at least.) Eigen questions are often asked on stackoverflow and there is also an FAQ.

The m is the 3 by 3 rotation matrix defined in the previous line. I admit that one letter variables names are not ideal :) The rxyz are XYZ intrinsic Euler angles created from the rotation matrix using the method eulerAngles.

You can also find an example to convert a position and Euler angles to an Eigen 3D pose (space transform / Isometry3d) in descartes/descartes_core/include/descartes_core/utils.h.

[blackhorsewu]

@JeroenDM Thank you very much for your replies and help. I cannot find where
zero_tol_pose
is declared and the compiler will not let me go!
Your pointer will be very much appreciated.

[JeroenDM]

zero_tol_pose is a name I gave to a variable I created. This line

CartTrajectoryPt zero_tol_pose(...);

creates an instance of the class CartTrajectoryPt. Notice that I added the line using namespace descartes_trajectory; because the class is defined inside a namespace. Without this I should have written descartes_trajectory::CartTrajectoryPt(...);. For more info, see the following great tutorials:

• namespaces
• classes

I'm happy to help you, but please remember that it is easier to help if you provide the example code and compiler error message.

[blackhorsewu]

@JeroenDM Thank you for your reply.
I asked the question is also because I saw that the test also use almost the same name
zero_tol_pos (not pose) and I actually used zero_tol_pos and the compiler complain that it was not declared anywhere. I thought it is a Descartes function.

[blackhorsewu]

@JeroenDM I should then declare the variable of type CartTrajectoryPt and creat it and fill in the content as you have taught me. Is that right?

[JeroenDM]

Indeed, as is done in makeTolerancedCartesianPoint used by the makePath function in tutorial 1.

Notice the slight difference with the example code I gave. A pointer to an CartTrajectoryPt instance is saved in a vector with the more generic type std::vector<descartes_core::TrajectoryPtPtr>. This approach is used in order to be able to save any type of trajectory point (AxialSymmetricPt, CartTrajectoryPt, ...) in the same vector.

Writing this, I noticed how essential good C++ knowledge is to effectively use this library, so it's probably a good idea to go through some C++ tutorials first if you want to avoid many frustrating compiler errors :)

[blackhorsewu]

@JeroenDM Thank you. I understand the difference between an object (or an instance of an object) is different from a pointer to an object. I will, in any case, go through some C++ tutorial to refresh my understanding again. Then I will try the code again.
Thank you for your patience and help.

[blackhorsewu]

@JeroenDM BTW, I found your paper on Evaluating Descartes as an algorithm to plan the Cartesian Path for Arc Welding Robot. That gives me more understanding of Descartes. Thank you also for the paper.

[JeroenDM]

:) No problem.

The numbers in the paper are quite out of date, as there have been large changes and improvements in the codebase over the years. But the explanation is still mostly valid, and I'm glad it is helpful :)

[blackhorsewu]

@JeroenDM I have the code for makeCartesianPoint as follows:
descartes_core::TrajectoryPtPtr makeCartesianPoint
(const Eigen::Isometry3d& pose, double dt)
{
using namespace descartes_core;
using namespace descartes_trajectory;

CartTrajectoryPt zero_tol_pose;

// Added on 11 July 2020 after receiving Jeroen's advice
Eigen::Vector3d t = pose.translation();
Eigen::Matrix3d m = pose.rotation();
Eigen::Vector3d rxyz = m.eulerAngles(0, 1, 2);

return TrajectoryPtPtr( new
CartTrajectoryPt(
zero_tol_pose(
TolerancedFrame(
pose,
ToleranceBase::zeroTolerance(t(0), t(1), t(2)),
ToleranceBase::zeroTolerance(rxyz(0),rxyz(1),rxyz(2))
), // end TolerancedFrame
0, // pos_increment
0 // orient_increment
) // end zero_tol_pose
// , TimingConstraint(dt)
) end CartTrajectoryPt
// TimingConstraint(dt)
);
}

And get the error message from catkin_make:
In function ‘descartes_core::TrajectoryPtPtr makeCartesianPoint(const Isometry3d&, double)’:
/home/victor/roboweld/src/roboweld_core/src/roboweld_motion.cpp:291:17: error: no match for call to ‘(descartes_trajectory::CartTrajectoryPt) (descartes_trajectory::TolerancedFrame, int, int)’
) // end zero_tol_pose
^
Would you please be so kind to help point out the problem. Thank you for your help.

[blackhorsewu]

@JeroenDM
I have solved the problem by the following:
Eigen::Vector3d t = pose.translation();
Eigen::Matrix3d m = pose.rotation();
Eigen::Vector3d rxyz = m.eulerAngles(0, 1, 2);

CartTrajectoryPt
zero_tol_pose(TolerancedFrame(
pose,
ToleranceBase::zeroTolerance(t(0), t(1), t(2)),
ToleranceBase::zeroTolerance(rxyz(0),rxyz(1),rxyz(2))
), // end TolerancedFrame
0, // pos_increment
0, // orient_increment
TimingConstraint(dt)
);

return TrajectoryPtPtr(new CartTrajectoryPt(zero_tol_pose) );

However, after I have used the above code for zero tolerance, Descartes does not always find the path for me. It is about 2 in 5 chance it can find the path but 3 out of 5 will failed to find a path. Is there any way to improve the probability of finding a path?

[JeroenDM]

When returning the TrajectoryPtPtr directly, you do not give it a name. The zero_tol_pose in the return statement is incorrect. Or explained from another point of view: you created a variable zero_to_pose of type CartTrajectoryPt. Then you try to use it as a function in the return statement: zero_tol_pose(...), which is not possible.

Note that you can format and highlight code in GitHub comments, which makes it easier to read.

descartes_core::TrajectoryPtPtr makeCartesianPoint(const Eigen::Isometry3d &pose, double dt)
{
    using namespace descartes_core;
    using namespace descartes_trajectory;

    // Added on 11 July 2020 after receiving Jeroen's advice
    Eigen::Vector3d t = pose.translation();
    Eigen::Matrix3d m = pose.rotation();
    Eigen::Vector3d rxyz = m.eulerAngles(0, 1, 2);

    return TrajectoryPtPtr(new CartTrajectoryPt(
        TolerancedFrame(
            pose,
            ToleranceBase::zeroTolerance(t(0), t(1), t(2)),
            ToleranceBase::zeroTolerance(rxyz(0), rxyz(1), rxyz(2))), // end TolerancedFrame
        0,                                                            // pos_increment
        0                                                             // orient_increment
        )                                                             // end CartTrajectoryPt
    );
}

[JeroenDM]

However, after I have used the above code for zero tolerance, Descartes does not always find the path for me. It is about 2 in 5 chance it can find the path but 3 out of 5 will failed to find a path. Is there any way to improve the probability of finding a path?

As there is no tolerance allowed on the end-effector, there are not many options to move along a path. (Depending on how many degrees of freedom the robot has.) So I would expect that some paths work and others don't. The only solution for paths that don't is to move the workpiece or the robot I think. But for exactly the same path and tolerances, Descartes should always return exactly the same solution, as far as I understand. It is not a probabilistic planner.

[blackhorsewu]

@JeroenDM
Thank you for all your help.
I was using exactly the same path and zero tolerances but it does not always give me a solution and some times it is up elbow and some times down elbow even when it gives me a solution!
Anyway to trackdown the problem?

[JeroenDM]

I'm not sure, but I suspect the randomness comes from the KDL inverse kinematics solver and MoveItStateWrapper, which maybe returns different solutions each time. If it does not return all solutions (for a 6 dof robot at least) all the time, that could explain the variation. So that's where I would start looking.

[blackhorsewu]

@JeroenDM
In that case would it help if I go back to use ur_kinematics/UR5KinematicsPlugin and IkFastMoveItStateAdapter?

[JeroenDM]

In that case would it help if I go back to use ur_kinematics/UR5KinematicsPlugin and IkFastMoveItStateAdapter?

If you can get it to work for your case, it is certainly a good idea.

[blackhorsewu]

@JeroenDM
Do you have any idea of :
https://moveit.ros.org/moveit!/ros/descartes/2019/04/12/moveit-descartes.html
and how to use this?

[JeroenDM]

I've tried that one out in the past. The wrapper makes it a lot easier to use, but not as flexible. I would certainly recommend trying it out for your use case. For the documentation:

Check it out on GitHub!

[blackhorsewu]

Thank you very much. I am trying it out now.

[blackhorsewu]

@JeroenDM
I have seen in the ROS-Industrial training, there is a "Descartes_Planning_and_Execution". They have something called "seed pose" before planning. I know that is not your work. However, do you know what that is and would it help in finding a path? If yes, how to use it?

[JeroenDM]

I don't know what it is. (Maybe it is related to this.) I think it is best to ask this in the repository of these tutorials: https://github.com/ros-industrial/industrial_training. Don't forget to add a link to the specific part of the tutorial you have a question about.

[blackhorsewu]

@JeroenDM I am reading the http://wiki.ros.org/descartes/Tutorials/Robot%20Welding%20With%20Descartes
There it mentioned
git clone https://github.com/JeroenDM/descartes.git
and said
This custom has an extra necessary commit to enable collision detection. This functionality is not yet correctly implemented in the most recent indigo version of Descartes.
I wonder is this still true and MoveIt and Descartes have not updated their versions to cope with collision detection?
Thank you very much.

[JeroenDM]

I do not think the changes we did a couple of years ago are still relevant or compatible with the current implementation. For the latest discussion regarding collision checking see ros-industrial-consortium/descartes#237.

But I think this is getting off the topic in the context of the original question in this issue. It would be clearer if you created a separate issue for specific problems you have with collision checking. Then it is easier for other people to jump in and help :)

(And maybe close this issue if you're original question was answered.)