Reusability of OpenManipulator for a Project & Simulation

Hi,

I was informed by Robotis-USA to inquire here with the hopes that the open source team could shed some light on my question.

An internal application we're developing (not sold, just for internal testing) will be using three dynamixel pro-pluses for a particular 3D articulation. I can speak more to the application if necessary, but it is fairly simple. One of Robotis' technical specialists pointed us to ROS as a means to speed up development and testing while waiting for the motors to arrive.

I've read through the ROS Robot Programming book (written by the Turtlebot3 developers), focusing mostly on the OpenManipulator (OM) example. I was able to get the OM simulation to articulate in Gazebo using the open_manipulator_control_gui after some effort.

My hope is to now leverage as much as I can of the open manipulator source code, refactoring it for our internal application application in order to test out some ideas before committing to the final design. I am short on time so I need to be able to have something under simulation within ~2 weeks, which is why I hope to re-use much of the OM code.

Browsing the OM projects source code has lead me to believe the set up will be a lot more complicated than originally thought. Stated differently, I was originally lead to believe that once the description was set up (urdf, importing stl files, joint, and linkage setup), that the simulation would help focus on tuning maneuver in 3D-space. There appears to be a lot of dynamics/kinematics code needed before I could even get to that point.

The key modules I'm speaking to are:

open_manipulator_controller/ appears to set up services to route requests to change/update the OM trajectory. Consequently, most if not all behavior funnels through callbacks associated to the advertisers which issue open_manipulator_.drawingTrajectoryMove() or open_manipulator_.jointTrajectoryMoveToPresentValue(), specifying the angle/position and the required time to complete the maneuver, when someone publishes to the topic. At first glance, this appears to be directly reusable as there doesn't appear to be anything specific to the OM mechanics.
open_manipulator_libs/ contains a lot of code which I think most could be reused directly, aside from the drawing shapes code. I think the big question for me is the re-usability of the Kinematics module. The OpenManipulator module in this path appears to be similar to the URDF setup files in the description so this would be need to be rewritten.
robotis_manipulator/ is probably the puzzling. There's the main cpp file, the math cpp file, and the trajectory cpp file, all of which I'm not sure how it fits into the overall picture other than "it helps things work". My gut is telling me this module would need to be rewritten in its entirety.

If I am to work on a fresh design of our own, will modules such as the kinematics or trajectory need to be written from scratch for use in our application? Per what I've covered above, or if I've left any module out, which of them would need to be written to be tailored to our application? The specialist we spoke to made it sound like the simulation development and testing would be focused on tuning the robotic maneuver, whereas this feels several weeks away if I need to write my own kinematic and trajectory modules, in addition to our own applications "robotis_manipulator".

Feel free to reach me at amanghi {@} orthalign {.} com for a direct response.

Hello,

We have an example of Open Manipulator customization here.

The main changes are the meshes, the urdf and the robot model cpp description in open_manipulator_libs/src/OpenManipulator.cpp (which is mainly the same as the urdf).

https://github.com/ROBOTIS-GIT/open_manipulator/blob/3b575b4c6b4c0212713c1e71a74da23e9192c227/open_manipulator_libs/src/OpenManipulator.cpp#L26-L73

In the case above we continue using 4 Dynamixel-XM430 (the same as the original Open Manipulator package), so you may need some more changes to adapt to the three-links structure and to the Dynamixel Pro+ (although if I'm not mistaken the dynamixel-workbench being used here is also compatible to Pro+).

Also, this package had a lot of updates (and should be having a new one soon in the feature_dynamics branch) since the ROS book was released, so make sure to keep track on the latest documentation in http://emanual.robotis.com/docs/en/platform/openmanipulator/

About the subpackages:

open_manipulator_controller/ is responsible for the ROS interface provided to the end-user. As you say, should be able to be reusable without major issues (you probably will need to change the angles of the home pose and gripper open/close).
open_manipulator_libs/ is probably where the adaptation work should be focused on, especially the OpenManipulator.cpp. I want to believe that the Kinematics.cpp was written in a form that it adaptively deals with the chain defined in OpenManipulator.cpp, so if you fix the addJoint and addTool in the initManipulator the Kinematics deals with the rest (will need some test/confirmation on this, though).
robotis_manipulator/ is designed as a common package for all manipulators using Dynamixel, so should (ideally) not include any specific coding and work just as it is for customized platforms.

@gpwhd07 @routiful Please correct me if I'm mistaken

Hi @orthalign-ameen
and thanks @Affonso-Gui

@Affonso-Gui is right. we design that modify the only open_manipulator_lib package according to modifying manipulator. And we updated new version of open manipulator package (feature_dynamics branch to master branch) yesterday.

If you want to custom the platform, you need to create a new manipulator class that inherits from RobotisManipulator class by refer 'open_manipulator_lib/open_manipulator.h' and 'cpp' files.

Dimension You have to set parameter of your manipulator by using addJoint() and addTool() function by refer initOpenManipulator() function.

/*****************************************************************************
  ** Initialize Manipulator Parameter 
  *****************************************************************************/
  addWorld("world",   // world name
           "joint1"); // child name

  addJoint("joint1",  // my name
           "world",   // parent name
           "joint2",  // child name
           math::vector3(0.012, 0.0, 0.017),               // relative position
           math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // relative orientation
           Z_AXIS,    // axis of rotation
           11,        // actuator id
           M_PI,      // max joint limit (3.14 rad)
           -M_PI);    // min joint limit (-3.14 rad)

  addJoint("joint2",  // my name
           "joint1",  // parent name
           "joint3",  // child name
           math::vector3(0.0, 0.0, 0.0595),                // relative position
           math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // relative orientation
           Y_AXIS,    // axis of rotation
           12,        // actuator id
           M_PI_2,    // max joint limit (1.67 rad)
           -2.05);    // min joint limit (-2.05 rad)

  addJoint("joint3",  // my name
           "joint2",  // parent name
           "joint4",  // child name
           math::vector3(0.024, 0.0, 0.128),               // relative position
           math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // relative orientation
           Y_AXIS,    // axis of rotation
           13,        // actuator id
           1.53,      // max joint limit (1.53 rad)
           -M_PI_2);  // min joint limit (-1.67 rad)

  addJoint("joint4",  // my name
           "joint3",  // parent name
           "gripper", // child name
           math::vector3(0.124, 0.0, 0.0),                 // relative position
           math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // relative orientation
           Y_AXIS,    // axis of rotation
           14,        // actuator id
           2.0,       // max joint limit (2.0 rad)
           -1.8);     // min joint limit (-1.8 rad)

  addTool("gripper",  // my name
          "joint4",   // parent name
          math::vector3(0.126, 0.0, 0.0),                 // relative position
          math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // relative orientation
          15,         // actuator id
          0.010,      // max gripper limit (0.01 m)
          -0.010,     // min gripper limit (-0.01 m)
          -0.015);    // Change unit from `meter` to `radian`

kinematics if your manipulator have serial linkage structure, you can use kinematics class (SolverUsingCRAndSRJacobian()) directly. If your manipulator dof have less of 6, you may need to modify the kinematics class because inverse kinematics cannot be solved about target orientation of the manipulator.

  /*****************************************************************************
  ** Initialize Kinematics 
  *****************************************************************************/
  kinematics_ = new kinematics::SolverCustomizedforOMChain();
//  kinematics_ = new kinematics::SolverUsingCRAndSRPositionOnlyJacobian();
  addKinematics(kinematics_);

Dynamixel (joint actuator) I think that you can use dynamixel.h without any modification if you use "dynamixel pro" only. you just need to set control mode or something you want to set.

/*****************************************************************************
** Initialize Joint Actuator
*****************************************************************************/
// actuator_ = new dynamixel::JointDynamixel();
actuator_ = new dynamixel::JointDynamixelProfileControl(control_loop_time);

// Set communication arguments
STRING dxl_comm_arg[2] = {usb_port, baud_rate};
void *p_dxl_comm_arg = &dxl_comm_arg;

// Set joint actuator id
std::vector<uint8_t> jointDxlId;
jointDxlId.push_back(11);
jointDxlId.push_back(12);
jointDxlId.push_back(13);
jointDxlId.push_back(14);
addJointActuator(JOINT_DYNAMIXEL, actuator_, jointDxlId, p_dxl_comm_arg);

// Set joint actuator control mode
STRING joint_dxl_mode_arg = "position_mode";
void *p_joint_dxl_mode_arg = &joint_dxl_mode_arg;
setJointActuatorMode(JOINT_DYNAMIXEL, jointDxlId, p_joint_dxl_mode_arg);

Dynamixel (tool actuator) you need to modify the tool actuator class according to your tool actuator.

/*****************************************************************************
** Initialize Tool Actuator
*****************************************************************************/
tool_ = new dynamixel::GripperDynamixel();

uint8_t gripperDxlId = 15;
addToolActuator(TOOL_DYNAMIXEL, tool_, gripperDxlId, p_dxl_comm_arg);

// Set gripper actuator control mode
STRING gripper_dxl_mode_arg = "current_based_position_mode";
void *p_gripper_dxl_mode_arg = &gripper_dxl_mode_arg;
setToolActuatorMode(TOOL_DYNAMIXEL, p_gripper_dxl_mode_arg);

// Set gripper actuator parameter
STRING gripper_dxl_opt_arg[2];
void *p_gripper_dxl_opt_arg = &gripper_dxl_opt_arg;
gripper_dxl_opt_arg[0] = "Profile_Acceleration";
gripper_dxl_opt_arg[1] = "20";
setToolActuatorMode(TOOL_DYNAMIXEL, p_gripper_dxl_opt_arg);

gripper_dxl_opt_arg[0] = "Profile_Velocity";
gripper_dxl_opt_arg[1] = "200";
setToolActuatorMode(TOOL_DYNAMIXEL, p_gripper_dxl_opt_arg);

Custom Trajectory I think that this class can be used directly. If you want to add some other trajectory motion you can make new class refer our class.

/*****************************************************************************
  ** Initialize Custom Trajectory
  *****************************************************************************/
  custom_trajectory_[0] = new custom_trajectory::Line();
  custom_trajectory_[1] = new custom_trajectory::Circle();
  custom_trajectory_[2] = new custom_trajectory::Rhombus();
  custom_trajectory_[3] = new custom_trajectory::Heart();

  addCustomTrajectory(CUSTOM_TRAJECTORY_LINE, custom_trajectory_[0]);
  addCustomTrajectory(CUSTOM_TRAJECTORY_CIRCLE, custom_trajectory_[1]);
  addCustomTrajectory(CUSTOM_TRAJECTORY_RHOMBUS, custom_trajectory_[2]);
  addCustomTrajectory(CUSTOM_TRAJECTORY_HEART, custom_trajectory_[3]);

Thank you both for the reply, @gpwhd07 and @Affonso-Gui . It was very helpful and insightful.

Something I should have mentioned in my initial post is that the robot we are creating is not a manipulator. It will use the three dynamixel pro pluses, but there is no gripper so using open_manipulator isn't a direct 1:1 mapping. I've included a diagram of the model below for clarity. The three motors are attached to one another (with the third attached to the mounting bracket, closely resembling the FRP42‑H120K). As far as the joints and linkages go, the part of the model that looks like a rail can be ignored for now.

In a separate _description project, loading my STL models into Gazebo caused them to fall apart/fall down (I believe due to gravity) once the simulation started. With your instructions, I rewrote the _description project to be more inline with how open_manipulator_description did it, using xarco for the urdf and gazebo xml files.

I also updated the open_manipulator_lib and was able to get my model's description loaded into Gazebo (which is where the screenshot above came from). This also included only specifying 3 dynamixel ID's, creating a "fake_gripper" tool (see below), and renaming all of the specified joints to match my urdf.xacro descriptions. The simulation no longer falls apart when run which is an improvement, so I've moved onto trying to control the robot from open_manipulator_control_gui.

  addJoint("joint_base_to_link1", // my name
           "world", // parent name
           "joint_link1_to_link2", // child name
           RM_MATH::makeVector3(0.0, 0.0, 0.0), // relative position
           RM_MATH::convertRPYToRotation(0.0, 0.0, 0.0), // relative orientation
           Y_AXIS, // axis of rotation
           11,     // actuator id
           M_PI*(7.0f/25.0f),   // max joint limit (50 degrees)
          -M_PI*(7.0f/25.0f));  // min joint limit (50 degrees)

  addJoint("joint_link1_to_link2", // my name
           "joint_base_to_link1", // parent name
           "joint_link2_to_link3", // child name
           RM_MATH::makeVector3(0.0, 0.0, 0.0), // relative position
           RM_MATH::convertRPYToRotation(0.0, 0.0, 0.0), // relative orientation
           Y_AXIS, // axis of rotation
           12,     // actuator id
           M_PI*(7.0f/25.0f),   // max joint limit (50 degrees)
          -M_PI*(7.0f/25.0f));  // min joint limit (50 degrees)

  addJoint("joint_link2_to_link3", // my name
           "joint_link1_to_link2", // parent name
           "fake_gripper",   // child name
           RM_MATH::makeVector3(0.0, 0.0, 0.0), // relative position
           RM_MATH::convertRPYToRotation(0.0, 0.0, 0.0), // relative orientation
           -Y_AXIS, // axis of rotation
           13,     // actuator id
           M_PI*(7.0f/25.0f),   // max joint limit (50 degrees)
          -M_PI*(7.0f/25.0f));  // min joint limit (50 degrees)

  addTool("fake_gripper",   // my name
          "joint_link2_to_link3", // parent name
          RM_MATH::makeVector3(0.0, 0.0, 0.0), // relative position
          RM_MATH::convertRPYToRotation(0.0, 0.0, 0.0), // relative orientation
          15,     // actuator id
          0.010,  // max gripper limit (0.01 m)
          -0.010, // min gripper limit (-0.01 m)
          -0.015); // Change unit from `meter` to `radian`

I've spent most of today trying to get the open_manipulator_control_gui node to communicate with Gazbeo but I did not have any luck. Could it be related to the fact that:

There is no gripper, even though open_manipulator requires one
- I've "faked it in open_manipulator.cpp by adding a tool which will end up doing nothing, and I planned on having no controls fed into it.
There are only 3 DoF, but it sounds like the Kinematics.cpp library needs at least 6.
- So I have to then write my own Kinematics framework?

I haven't been able to publish to the /open_manipulator/joint*_position/ topics from the command line or the controls gui (after some modifications). I know this is a very wide net to cast but any insight would be appreciated. I want to be certain my current approach will yield in the ability to simulate and control the robot. This is all new territory for me so I cannot say for certain I'm on the right track.

open_manipulator_gazebo.launch

<launch>
  <!-- These are the arguments you can pass this launch file, for example paused:=true -->
  <arg name="paused" default="true"/>
  <arg name="use_sim_time" default="true"/>
  <arg name="gui" default="true"/>
  <arg name="headless" default="false"/>
  <arg name="debug" default="false"/>

  <!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(find open_manipulator_gazebo)/worlds/empty.world"/>
    <arg name="debug" value="$(arg debug)" />
    <arg name="gui" value="$(arg gui)" />
    <arg name="paused" value="$(arg paused)"/>
    <arg name="use_sim_time" value="$(arg use_sim_time)"/>
    <arg name="headless" value="$(arg headless)"/>
  </include>

  <!-- Load the URDF into the ROS Parameter Server -->
  <param name="robot_description"
   command="$(find xacro)/xacro --inorder '$(find dynamixel_simulation_description)/urdf/dynamixel_simulation.urdf.xacro'"/>

  <!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
  <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
     args="-urdf -model dynamixel_simulation -z 0.0 -param robot_description"/>

  <!-- ros_control robotis manipulator launch file -->
  <include file="$(find open_manipulator_gazebo)/launch/open_manipulator_controller.launch"/>
</launch>

open_manipulator_controller.yaml


pen_manipulator:
  # Publish all joint states -----------------------------------
  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 1000

  # Position Controllers ---------------------------------------
  joint1_position:
    type: position_controllers/JointPositionController
    joint: joint_base_to_link1

  joint2_position:
    type: position_controllers/JointPositionController
    joint: joint_link1_to_link2

  joint3_position:
    type: position_controllers/JointPositionController
    joint: joint_link2_to_link3

  gripper_position:
    type: position_controllers/JointPositionController
    joint: fake_gripper

Please let me know if there is any other info you might need.

Hello,

Glad to see we are having some progress. A few more questions related to this problem:

Have you launched the controller before the gazebo node? http://emanual.robotis.com/docs/en/platform/openmanipulator/#gazebo-simulation

roslaunch open_manipulator_controller open_manipulator_controller.launch use_platform:=false

Do you receive any messages in the logging area of open_manipulator_control_gui?

Yes so the example as followed in the e-manual works out of the box with no issue. I was able to navigate to the initial and home poses, and open/close the gripper all in Gazebo. My order of launching is:

1) roscore 2) open_manipulator_controller node (w/ use_platform:=false) 3) gazebo node 4) open_manipulator_control_gui node

When I start the timer (via Timer Start) in my sandbox, the OM states to the left of the button do not update like they do in the unmodified sandbox (e-manual example). There seems to be a disconnect in the command/topic communication which I'm not sure how to pinpoint.

Your problem seems to be with the missing tool frame.

For now I got a three-link simulation working by using a super realistic dummy gripper (essentially the same as the original one): https://github.com/Affonso-Gui/open_manipulator/tree/test_4l https://github.com/Affonso-Gui/open_manipulator_simulations/tree/test_4l

test4l

Moveit demo is still not adapted, but should be fairly straightforward (you might need to add up dummy links until you have a virtually 6DoF chain https://answers.ros.org/question/152591/kdl-kinematics-solver-limitations/)

Anyways, this should work for simple simulations but will probably crash with the real robot, since there is no motor to communicate with for the tool joint. I'm not sure how many changes will be needed to make this adaptation, but in the worst case it might go all back to the robotis_manipulator (in the good case it will be enough to turn off related publishers/ subscribers at open_manipulator_controller.

Making customization of this package easier and more intuitive is indeed an issue, and supporting gripper-less models might also be a nice add-up (although not being manipulators per se).

What I ended up doing was using the example OpenManipulator ecosystem but replacing the models/URDF with my own files, keeping the joint names, robot names, and all other names the same. I only kept the three joints and removed all gripper-related linkages aside from the first gripper link. I was able to get our robot model to appear in rviz and gazebo, and able to control it via the control gui at the joint level. I'm still working through some other specific-to-my-model URDF bugs, but all in all I think I can get this working. This experiment was to prove it CAN be done, which is nice.

I do believe that getting the correct motor definitions (using the pro+ motor's characteristics and not the default motor used in the OpenManipulator example) is vital. I still haven't figured this out so guidance would be appreciated. Once I have direction on this, I will consider this issue closed since it IS possible to use one's own models and joint definitions with the mostly-unmodified OpenManipulator example.

Hi @orthalign-ameen , @Affonso-Gui ,

First, I am happy with your success. You have finished almost step, and in your current source code, control using joint angles may be able to work successfully.

However, there are some missing parts in open_manipulator_lib. If you add this, I think you can also run commands at task space.

In initOpenManipulator() function of open_manipulator.cpp, you need to update the relative position, relative orientation, and axis of rotation in addition to the name of the link.

addJoint("joint_base_to_link1", // my name
           "world", // parent name
           "joint_link1_to_link2", // child name
           RM_MATH::makeVector3(0.0, 0.0, 0.0), // relative position
           RM_MATH::convertRPYToRotation(0.0, 0.0, 0.0), // relative orientation
           Y_AXIS, // axis of rotation
           11,     // actuator id
           M_PI*(7.0f/25.0f),   // max joint limit (50 degrees)
          -M_PI*(7.0f/25.0f));  // min joint limit (50 degrees)

  addJoint("joint_link1_to_link2", // my name
           "joint_base_to_link1", // parent name
           "joint_link2_to_link3", // child name
           RM_MATH::makeVector3(0.0, 0.0, 0.0), // relative position
           RM_MATH::convertRPYToRotation(0.0, 0.0, 0.0), // relative orientation
           Y_AXIS, // axis of rotation
           12,     // actuator id
           M_PI*(7.0f/25.0f),   // max joint limit (50 degrees)
          -M_PI*(7.0f/25.0f));  // min joint limit (50 degrees)

  addJoint("joint_link2_to_link3", // my name
           "joint_link1_to_link2", // parent name
           "fake_gripper",   // child name
           RM_MATH::makeVector3(0.0, 0.0, 0.0), // relative position
           RM_MATH::convertRPYToRotation(0.0, 0.0, 0.0), // relative orientation
           -Y_AXIS, // axis of rotation
           13,     // actuator id
           M_PI*(7.0f/25.0f),   // max joint limit (50 degrees)
          -M_PI*(7.0f/25.0f));  // min joint limit (50 degrees)

  addTool("fake_gripper",   // my name
          "joint_link2_to_link3", // parent name
          RM_MATH::makeVector3(0.0, 0.0, 0.0), // relative position
          RM_MATH::convertRPYToRotation(0.0, 0.0, 0.0), // relative orientation
          15,     // actuator id
          0.010,  // max gripper limit (0.01 m)
          -0.010, // min gripper limit (-0.01 m)
          -0.015); // Change unit from `meter` to `radian`

The above does not include addWorld(), but you need to add as following.

addWorld ("world", // world name
           "joint_base_to_link1"); // child name

Relative position is the relative distance of the link (my name) from the parent link (parent name). Relative orientation indicates the relative orientation of the link (my name) from the parent link (parent name). Axis of rotation is the rotation axis of joint in the coordinates of the link.

This information will already be specified in theurdf.xacro file. Below is an example of the open_manipulator.urdf.xacro file.

  <!-- Joint 1 -->
  <joint name="joint1" type="revolute">
    <parent link="link1"/>
    <child link="link2"/>
    <origin xyz="0.012 0.0 0.017" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit velocity="4.8" effort="1" lower="${-pi*0.9}" upper="${pi*0.9}" />
  </joint>

Here, xyz of <origin xyz = "0.012 0.0 0.017" rpy = "0 0 0" /> represents the relative position and rpy represents the relative orientation. And <axis xyz = "0 0 1" /> represents axis of rotation, in this case Z_AXIS.

If you modify the initManipulator() function that includes addJoint(), the OM states of the GUI will be updated without any modification of the kinematics, and the operation at the task space will also be performed.

@gpwhd07 , @Affonso-Gui Thank you for your help!

In initOpenManipulator() function of open_manipulator.cpp, you need to update the relative position, relative orientation, and axis of rotation in addition to the name of the link.

Yes these values have changed as I've been updating the URDF descriptor over the past several days. I believe I've finally gotten the link inertias set up correctly. I still need to update the controller lib source to reflect the modified values. Before I do this, however, I have one final question that remains unanswered.

How do I get the specifications of the Pro+ Dynamixels imported into the controller library and the Gazebo simulation? It's behavior/characteristics differ from the Dynamixels used in the Open_Manipulator in many ways. I'm not even sure if this matters or if I need this step. Guidance is appreciated. All I've found so far is the joints are specified via #define JOINT_DYNAMIXEL "joint_dxl".

I know there are additional fields in the URDF when specifying a revolute joint, such as the physical damps, and static friction. What else would be needed?

<joint name="joint1" type="revolute">
   ...
   <dynamics damping="0.0" friction="0.0"/>`
   ...
</joint>

Hello, @orthalign-ameen It is very good news.

First of all, if you use pro + without using multiple series of dynamixel, you can use the current JOINT_DYNAMIXEL class without any modification.

However, if you want to use multiple series in a manipulator, you have to modify JOINT_DYNAMIXEL class, because control tables of dynamixels differ according to it's series. About this control table, you can refer the e-manual of dynamixel. If you need to modify JOINT_DYNAMIXEL class, it is defined in open_manipulator_lib/dynamixel.cpp

Good. So all that's needed is to ensure the control table matches. It sounds like the simulation does not care about the physical properties of the Dynamixel other than what's defined in the <joint>.

Closing as resolved. Thank you for your help!

ROBOTIS-GIT / open_manipulator

Reusability of OpenManipulator for a Project & Simulation #84