Usability of a custom gripper in OpenManipulator-PRO with communication problems

[KaikeWesleyReis]

Hello engineers! I'm trying to insert a custom gripper in OpenManipulator-PRO. This custom gripper is a MX-28 Dynamixel with FR07-G101GM-Set. First, some statements:

• Was insert a DC conversor 24V to 12V in serial for MX-28 (I managed to change only Ground-Source for 12V and keeped the communication in serial in U2D2 with others the others dynamixels)
• MX-28 Protocol was updated from 1.0 to 2.0!
• I can control my MX-28 with Dynamixel Wizard 2.0 Successfully

Second, what I made? Following this other issue for OpenManipulator, I tried to insert my gripper as a new joint called joint7 because function addTool does not have a rotation axis argument (my gripper rotates in this y-axis). For this, I changed this code open_manipulator_pro_libs/open_manipulator_pro.cpp:

• Insert the new joint in function initOpenManipulator

  addJoint("joint6",   // my name
          "joint5",  // parent name
          "gripper", // child name
          math::vector3(0.123, 0.0, 0.0),               
          math::convertRPYToRotationMatrix(0.0, 0.0, 0.0),
          X_AXIS,    // axis of rotation
          6,         // actuator id
          M_PI,      // max joint limit (3.14 rad)
          -M_PI,     // min joint limit (-3.14 rad)
          1.0,       // coefficient
          1.4327573e-01,                                         
          math::inertiaMatrix(8.0870749e-05, 0.0, -1.0157896e-06,
                              7.5980465e-05, 0.0,
                              9.3127351e-05),                              
          math::vector3(4.4206755e-02, 3.6839985e-07, 8.9142216e-03)  
          );
  
  addJoint("joint7",   // my name
          "joint6",  // parent name
          "gripper", // child name
          math::vector3(0.0315, -0.017, 0.019),       
          math::convertRPYToRotationMatrix(0.0, -0.1396, 0.0),
          Y_AXIS,    // axis of rotation
          7,         // actuator id
          1.75,      // max joint limit (100 degrees -> 1.75 rad)
          0.14,      // min joint limit (8 degrees -> 0.14 rad)
          1.0,       // coefficient
          1.4327573e-01,                                                      
          math::inertiaMatrix(8.0870749e-05, 0.0, -1.0157896e-06,
                              7.5980465e-05, 0.0,
                              9.3127351e-05),                                 
          math::vector3(4.4206755e-02, 3.6839985e-07, 8.9142216e-03)         
          );
  
  int gripper_id = -1;
  if (with_gripper) gripper_id = 99;
  
  addTool("gripper",  // my name
        "joint7",   // parent name
        math::vector3(0.1223, 0.0, 0.0),                    // relative position
        // math::vector3(0.150, 0.0, 0.0),                  // relative position
        math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // relative orientation
        gripper_id,  // actuator id
        1.1351,     // max gripper limit (0.01 m)
        -0.001,    // min gripper limit (-0.01 m)
        1.0,    // Change unit from `meter` to `radian`
        3.2218127e-02 * 2,                                                   
        math::inertiaMatrix(9.5568826e-06, 2.8424644e-06, -3.2829197e-10,
                            2.2552871e-05, -3.1463634e-10,
                            1.7605306e-05),                                  
        math::vector3(0.028 + 8.3720668e-03, 0.0246, -4.2836895e-07)       
        );

• In the same function, added a new jointDxlId.push_back(7)

  /*****************************************************************************
  ** Initialize Kinematics 
  *****************************************************************************/
  kinematics_ = new kinematics::SolverUsingCRAndGeometry();
  
  addKinematics(kinematics_);
  
  // Set kinematics arguments
  void *p_with_gripper = &with_gripper;
  setKinematicsOption(p_with_gripper);
  
  if(using_actual_robot_state)
  {
  /*****************************************************************************
  ** 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(1);
  jointDxlId.push_back(2);
  jointDxlId.push_back(3);
  jointDxlId.push_back(4);
  jointDxlId.push_back(5);
  jointDxlId.push_back(6);
  jointDxlId.push_back(7);
  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);
  
  /****************************************************************************

• As was said in the related issue, I don't need to change my Inverse Kinematics because DOF still higher than six.

When I launch open_manipulator_pro_controller.launch with or without MoveIt! (I made a specific package for this customization, where group arm have now seven joints) the /open_manipulator_pro/joint_states for my new gripper is equal for 63935.53125 regardless his position (manually changing). I want to know what I'm doing wrong and why this higher value for my joint_state.

Best regards from Brazil,

[JaehyunShim]

@KaikeWesleyReis

You will only need to modify the elements you give to the addTool function (relative position and max&min gripper limit). Remove all the other modifications you made on the file and test it again.

Basically, we do not support users' own customised robots, so in this kind of case, you might want to compare om-pro.cpp file and om.cpp file (for the om-x model) and figure out what you have to change to build your own program on your own.

Regards, Ryan

[KaikeWesleyReis]

@rjshim Hi rjshim, I made the changes that you said, but I have some doubts about your gripper, could you please tell me:

• Its a revolute motor? It's rotate in Y AXIS?
• What means the line 1.0, // Change unit from meter to radian in addTool? There is other options?

[JaehyunShim]

@KaikeWesleyReis

Please check this link for the gripper model. As for the usage of the addTool function, this link might be of your interest.

Ryan

[KaikeWesleyReis]

@rjshim thanks for your help! I made the changes in addTool function open_manipulator_pro.cpp for my custom gripper that generates some strange values in joint_state topic. The solution was to send a command through one of your service (goal_control) for my gripper that normalize the results! It is solved.

[JaehyunShim]

@KaikeWesleyReis

Great! Please close your issue if you don't have any relevant questions anymore. I will close this issue this time. Ryan