Robot Model Import Problems | Denavit-Hartenberg

[AlejoDiaz49]

Hello

I create a really simple program that import a JACO2 and move it. The first problem I have is that if the robot is displayed in the coordinate (0,0,0) some parts of the robot are not shown, so like in some of the examples I add 3 prismatic links to add movement in X,Y and Z. The robot is displayed and it moves but in every iteration this error appears

Gpu_voxels_helpers(Error) MetaPointCloud::getCloudNumber: The name z_translation is unknown Gpu_voxels_helpers(Error) MetaPointCloud::getCloudNumber: The name y_translation is unknown Gpu_voxels_helpers(Error) MetaPointCloud::getCloudNumber: The name x_translation is unknown

Also I am having some problems with the coordinate systems of the links. I adjust them in the models using CloudCompare and some of them are placed perfectly in the visualizer but there other that aren't. You can see here how the "Base" link is not in Z=0, it has like an offset that also the next link "Shoulder" has. Did someone has any problem like this one? How do you solve it?

Finally, How do you define the max and min values that the joint can reach, I tried to write them in radians in robot::JointValueMap min_joint_values and robot::JointValueMap max_joint_values, but it's not working

Here I let the code

#include <signal.h>
#include <gpu_voxels/GpuVoxels.h>

using namespace gpu_voxels;
gpu_voxels::GpuVoxelsSharedPtr gvl;

// We define exit handlers to quit the program:
void ctrlchandler(int)
{
    gvl.reset();
    exit(EXIT_SUCCESS);
}
void killhandler(int)
{
    gvl.reset();
    exit(EXIT_SUCCESS);
}

int main(int argc, char* argv[])
{
    signal(SIGINT, ctrlchandler);
    signal(SIGTERM, killhandler);

    // First we initialize gpu voxels
    icl_core::logging::initialize(argc, argv);
    gvl = GpuVoxels::getInstance();
    gvl->initialize(200, 200, 150, 0.01);

    // We add different maps with objects, to collide them
    gvl->addMap(gpu_voxels::MT_PROBAB_VOXELMAP,"Map");
    gvl->addMap(MT_BITVECTOR_VOXELMAP, "Robot");

    //3D models of the robot
    int Links = 9;
    std::vector<std::string> linknames(Links);
    std::vector<std::string> paths_to_pointclouds(Links-3);

    linknames[0] = "z_translation";
    linknames[1] = "y_translation";
    linknames[2] = "x_translation";

    linknames[3] = paths_to_pointclouds[0] = "kinova/JACOSpherical/BaseC.binvox";
    linknames[4] = paths_to_pointclouds[1] = "kinova/JACOSpherical/ShoulderC.binvox";
    linknames[5] = paths_to_pointclouds[2] = "kinova/JACOSpherical/Arm.binvox";
    linknames[6] = paths_to_pointclouds[3] = "kinova/JACOSpherical/Forearm.binvox";
    linknames[7] = paths_to_pointclouds[4] = "kinova/JACOSpherical/UpperWristS10.binvox";
    linknames[8] = paths_to_pointclouds[5] = "kinova/JACOSpherical/LowerWristS1.binvox";

    //Kinematics
    std::vector<robot::DHParameters> dh_params(Links);

    const double PI = 3.1416;
    const double D1 = 0.2755;
    const double D2 = 0.4100;
    const double D3 = 0.2073;
    const double D4 = 0.1038;
    const double D5 = 0.1038;
    const double D6 = 0.1600;
    const double e2 = 0.0098;

    dh_params[0] = robot::DHParameters(      0.0,    0.0,  0.0,  -PI/2,   0.0,  robot::PRISMATIC);
    dh_params[1] = robot::DHParameters(      0.0,  -PI/2,  0.0,  -PI/2,   0.0,  robot::PRISMATIC);
    dh_params[2] = robot::DHParameters(      0.0,   PI/2,  0.0,   PI/2,   0.0,  robot::PRISMATIC);

    dh_params[3] = robot::DHParameters(       D1,    0.0,  0.0,   PI/2,   0.0,  robot::REVOLUTE);
    dh_params[4] = robot::DHParameters(      0.0,   PI/2,   D2,     PI,   0.0,  robot::REVOLUTE);
    dh_params[5] = robot::DHParameters(      -e2,  -PI/2,  0.0,   PI/2,   0.0,  robot::REVOLUTE);
    dh_params[6] = robot::DHParameters( -(D3+D4),    0.0,  0.0,   PI/2,   0.0,  robot::REVOLUTE);
    dh_params[7] = robot::DHParameters(      0.0,    0.0,  0.0,   PI/2,   0.0,  robot::REVOLUTE);

    gvl->addRobot("Robot", linknames, dh_params, paths_to_pointclouds, true);

    std::size_t counter = 0;
    const float ratio_delta = 0.07;

    robot::JointValueMap min_joint_values;
    min_joint_values["z_translation"] = 0.0; // moves along the Z axis
    min_joint_values["y_translation"] = 0.5; // moves along the Y Axis
    min_joint_values["x_translation"] = 0.5; // moves along the X Axis

    min_joint_values["kinova/JACOSpherical/BaseC.binvox"] = 0.0;
    min_joint_values["kinova/JACOSpherical/ShoulderC.binvox"] = 0.0;
    min_joint_values["kinova/JACOSpherical/Arm.binvox"] = 0.0;
    min_joint_values["kinova/JACOSpherical/Forearm.binvox"] = 0.0;
    min_joint_values["kinova/JACOSpherical/UpperWristS10.binvox"] = 0.0;
    min_joint_values["kinova/JACOSpherical/LowerWristS1.binvox"] = 0.0;

    robot::JointValueMap max_joint_values;
    max_joint_values["z_translation"] = 0.0; // moves along the Z axis
    max_joint_values["y_translation"] = 0.5; // moves along the Y axis
    max_joint_values["x_translation"] = 0.5; // moves along the X Axis

    max_joint_values["kinova/JACOSpherical/BaseC.binvox"] = 1;
    max_joint_values["kinova/JACOSpherical/ShoulderC.binvox"] = 1;
    max_joint_values["kinova/JACOSpherical/Arm.binvox"] = 1;
    max_joint_values["kinova/JACOSpherical/Forearm.binvox"] = 1;
    max_joint_values["kinova/JACOSpherical/UpperWristS10.binvox"] = 1;
    max_joint_values["kinova/JACOSpherical/LowerWristS1.binvox"] = 1;

    robot::JointValueMap RobotJointValues;

    while(true)
    {
        LOGGING_INFO(Gpu_voxels, "Updating robot pose..." << endl);
        RobotJointValues = gpu_voxels::interpolateLinear(min_joint_values, max_joint_values, ratio_delta * counter++);
        gvl->setRobotConfiguration("Robot", RobotJointValues);
        gvl->insertRobotIntoMap("Robot", "Map", eBVM_OCCUPIED);

        gvl->visualizeMap("Map");
        usleep(100000);
        gvl->clearMap("Map");
    }
}

[cjue]

Hi Alejandro,

thanks for your report!

• the "name z_translation not known" bug probably stems from the fact that the virtual translation links don't have any related geometry and we therefore don't store them in a MetaPointCloud. The "ERROR" logging is a bit misleading here.

• regarding the placement of the robot in the visualizer: The urdf robot parser implicitly looks for a root link in the robot's urdf and uses that as the robot's origin. In your case this might well be one of the "_translation" links. You can easily move the origin anywhere you like by adding an additional link to the URDF and place it at the root of the robot tree.

• regarding joint value limits: what exactly is not working?

[AlejoDiaz49]

Hi Christian,

• I am not using the URDF loader. I understand that without ROS it cant be used since I didn't found any URDF parser so that when I compile the URDF support message is OK. Right now I am working in a PC that has ROS but in the future everything has to be working in a PC without ROS. If there is any way to use URDF without installing I would apprecieate if you tell me how.

• The problem with the joint value limits is that I put some limits but when the links rotate they just do it without caring about the limist. So i dont know if the limit are not in radians or if the way I am using them is wrong

• My main problem is that using the DH parameters way to import the robot, I am struggling to put the coordinate system in each link and the result is like in the image I sent, that there is like an error in the position of some links. Since I can't use URDF and define where is the coordinate system I am doing it adjusting the coordiante system position and orientation using a 3D tool (CloudCompare). I need to solve this.

Also, I forgot to tell you that in the CMakeLists.txt of the _example_how_tolink I had to add CUDA dependencies to make my programs compile. I dont know if you forgot it in the file.

Thanks in advanced

[Squelsh]

Hi Alejandro,

DH parameters are always a pain until you get them right ;) Therefore I prefer URDF.

I can not tell you with 100% certainty as I can not test it currently, but the URDF parsing (urdfdom library) is not longer a ROS package but went into the misc branch of the regular Ubuntu repositories in ~2014. Same for the other dependencies: FIND_PACKAGE(urdfdom) FIND_PACKAGE(orocos_kdl) FIND_PACKAGE(kdl_parser)

All other relevant code is directly in GPU-Voxels. ==> You should be able to use URDFs without ROS.

[Squelsh]

About the limits: The DH model does not care about limits. And limits you define in your code will be extrapolated by the interpolateLinear function, as soon as ratio_delta * counter is larger than 1.0.

[AlejoDiaz49]

Hi Andreas

I installed from repository orocos_kdl and urdfdom (urdfdom_headers and console_bridge were needed) and everything is OK but when I am installing kdl_parser it requieres or want to install a lot of ROS modules.

Also when I compile this repository the message that said what is missing to be able to use URDF and it says ROS, so at the end it seems like I need ROS

-- [WARNING] Building GPU-Voxels without URDF support. Could not find the following packages: -- kdl_parser -- ROS

I mean If it's not possible I suppose I will keep trying to adjust the models to DH. That was my original plan but it would be nice to use URDF directly

[cjue]

Hi, the comments in the central CMakeLists still suggest URDF support depends on ROS: # Dependencies for URDF loading (also requires ROS for kdl/frames.hpp)

This matches the CMakeLists of gpu_voxels_urdf_robot, which still lists all three as a requirement:

IF(urdfdom_FOUND AND orocos_kdl_FOUND AND kdl_parser_FOUND AND ROS_FOUND)

If anyone can test if ROS is indeed unneccessary in newer Ubuntu distributions, I would be happy to hear about it :)

[cjue]

I just had a look, and the required packages can be installed in a ROS-free Ubuntu 16.04: sudo apt install libkdl-parser0d libkdl-parser-dev liborocos-kdl1.3 liborocos-kdl-dev liburdfdom-dev liburdfdom-tools

gpu_voxels_urdf_robot needs a patch to remove the unneccesary reference to ROS. This will be part of the next release:

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 0fb9be9..d22575b 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -82,9 +82,9 @@ ELSE(ROS_FOUND)
   MESSAGE(STATUS "[WARNING] Building GPU-Voxels without ROS connections. ROS not found.")
 ENDIF(ROS_FOUND)

-IF(urdfdom_FOUND AND orocos_kdl_FOUND AND kdl_parser_FOUND AND ROS_FOUND)
+IF(urdfdom_FOUND AND orocos_kdl_FOUND AND kdl_parser_FOUND)
   MESSAGE(STATUS "[OK]      Building GPU-Voxels with URDF support. urdfdom, orocos and kdl_parser and ROS-kdl were found.")
-ELSE(urdfdom_FOUND AND orocos_kdl_FOUND AND kdl_parser_FOUND AND ROS_FOUND)
+ELSE(urdfdom_FOUND AND orocos_kdl_FOUND AND kdl_parser_FOUND)
   MESSAGE(STATUS "[WARNING] Building GPU-Voxels without URDF support. Could not find the following packages:")
   IF(NOT urdfdom_FOUND)
     MESSAGE(STATUS "    urdfdom")
@@ -95,10 +95,7 @@ ELSE(urdfdom_FOUND AND orocos_kdl_FOUND AND kdl_parser_FOUND AND ROS_FOUND)
   IF(NOT kdl_parser_FOUND)
     MESSAGE(STATUS "    kdl_parser")
   ENDIF(NOT kdl_parser_FOUND)
-  IF(NOT ROS_FOUND)
-    MESSAGE(STATUS "    ROS")
-  ENDIF(NOT ROS_FOUND)
-ENDIF(urdfdom_FOUND AND orocos_kdl_FOUND AND kdl_parser_FOUND AND ROS_FOUND)
+ENDIF(urdfdom_FOUND AND orocos_kdl_FOUND AND kdl_parser_FOUND)

 #IF(OPENNI2_FOUND)
 #  MESSAGE(STATUS "[OK]      Building GPU-Voxels with Kinect support. OpenNI2 was found.")
diff --git a/src/gpu_voxels/robot/urdf_robot/CMakeLists.txt b/src/gpu_voxels/robot/urdf_robot/CMakeLists.txt
index fea4fd1..14c64a2 100644
--- a/src/gpu_voxels/robot/urdf_robot/CMakeLists.txt
+++ b/src/gpu_voxels/robot/urdf_robot/CMakeLists.txt
@@ -11,7 +11,7 @@
 #----------------------------------------------------------------------

 #------------- gpu_voxels_urdf_robot libaray -----------------------
-IF(urdfdom_FOUND AND orocos_kdl_FOUND AND kdl_parser_FOUND AND ROS_FOUND)
+IF(urdfdom_FOUND AND orocos_kdl_FOUND AND kdl_parser_FOUND)
   ICMAKER_SET("gpu_voxels_urdf_robot" IDE_FOLDER ${GPU_VOXELS_IDE_FOLDER})

   ICMAKER_GLOBAL_CPPDEFINES(-D_BUILD_GVL_WITH_URDF_SUPPORT_)
@@ -59,7 +59,6 @@ IF(urdfdom_FOUND AND orocos_kdl_FOUND AND kdl_parser_FOUND AND ROS_FOUND)
     kdl_parser
     orocos_kdl
     Boost_FILESYSTEM
-    ROS
     )

@@ -73,4 +72,4 @@ IF(urdfdom_FOUND AND orocos_kdl_FOUND AND kdl_parser_FOUND AND ROS_FOUND)
     ../robot_interface.h
   )

-ENDIF(urdfdom_FOUND AND orocos_kdl_FOUND AND kdl_parser_FOUND AND ROS_FOUND)
+ENDIF(urdfdom_FOUND AND orocos_kdl_FOUND AND kdl_parser_FOUND)