Controller tuning for custom robot

[Nielsencu]

I managed to import a Kinova Gen3 robot arm with the Robotiq 2F-85 into the simulation. I also followed the custom robots guide stated below

https://maniskill.readthedocs.io/en/latest/user_guide/tutorials/custom_robots.html

I've verified that my robot with pd_joint_delta_pos controller and random action moves similar to the video shown and also the pd_joint_pos controller maintains the rest keyframe accordingly.

However, when I trained the robot on StackCube-v1 task it approaches the cube but doesn't really grip it and it behaves quite weirdly.

I have attached a video showing the training (step 0 and step 104).

https://github.com/user-attachments/assets/298341ff-dc29-41ef-aa58-982bd7141346

https://github.com/user-attachments/assets/0b17e191-ee85-4d0e-8c85-230bd5c56f21

Do you have some other tips for tuning the P, D, and also force limit values for the arm and the gripper that is more effective e.g tuning the PD values real-time with the simulator interactively? I also realized for all grippers, such as fetch and panda they have the same PD and force limit coefficients, does that mean for my robot it should also be the same?

I also found this issue https://github.com/haosulab/ManiSkill/issues/71 which tried to use robotiq gripper. Can I also check if it was succesfully implemented?

[StoneT2000]

the P, D and force limit values in simulation don't matter that much, you can probably just copy the values for fetch/panda for your implementation. They only matter if you want to do sim2real transfer then you need to system ID by checking if an action in sim corresponds to the same behavior in the real world.

I assume you are using PPO to train. Maybe first start with the PushCube and PickCube tasks. StackCube is actually much harder than it looks (most RL algorithms solve this task slower than most existing robot RL benchmarks).

Another thing, it looks like the robot is initialized at the middle of the table. This might make the task too hard to solve. If you are using the stack cube reward function, it encourage the gripper to move towards the red cube first, then grasp it. However the current position of the robot might make this too hard since the cube is often very close to the robot base.

Another thing is video 3.mp4 the full video? As in at the start of the evaluation episode it looks like that?

[Nielsencu]

Thank you for your suggestion. Indeed, I realized that the PD values are not the issue.

I finally found the culprit and it is due to the end effector (Robotiq-2F-85), it has a peculiar configuration, having 6 actively controlled joints compared to the conventional 2-joint EE such as one's used for Panda.

In Gazebo, I used the joint state publisher gui to control the gripper for open and closing and it works fine. Video below shows manually moving just the finger_joint through the GUI.

https://github.com/user-attachments/assets/426692cb-7e39-48c2-a8a1-18c8ac05cbd5

However, when exporting it from .xacro to urdf and loading the URDF into Maniskill, it behaves weirdly. Video below is again manually moving the finger_joint through the slider, but robot goes haywire.

https://github.com/user-attachments/assets/78010224-d75a-4a9a-85fd-bd974dc9eadd

Note that the finger_joint is a mimic_joint, which moves other joints that have a mimic tag referencing the finger_joint. Perhaps this is what's causing the problem? Here's the URDF if it's helpful to debug. gen3_robotiq_2f_85_ori.zip

One thing though the robotiq (2f-85) gripper was used in ManiSkill few years back, and I compared my URDF with the one in the repo below:

https://github.com/haosulab/cvpr-tutorial-2022/blob/master/assets/robotiq_2f_85_gripper_visualization/robotiq_85_original.urdf

It's also seem to be using mimic joint, but all the inertial component in the links got removed. I'm afraid disabling this might not translate well for sim-to-real.

Nonetheless,I tried removing the inertial links from the robot, however, still no good results (shown below).

https://github.com/user-attachments/assets/bf251d69-6865-4bb9-9498-aee3be8a56f0

When all joints in the gripper were disabled, the robot could move normally.

https://github.com/user-attachments/assets/cb53c8a2-2ce9-463c-a468-c777c3ea4429

I used Gen3 with all gripper joints disabled and tested it on the PushCube task and initialized the robot closer to the edge of the table, following your suggestion, and it can learn the task by using its arm to push the cube to target destination although not 100% success rate after 10 million steps.

I also tested the xmate3_robotiq robot but the gripper seems to also be disconnected.

https://github.com/user-attachments/assets/6fc6d9c9-52df-4e04-ad5e-06d87edb98a8

Hope you can give some advice regarding this! Let me know if you need more information, and thank you so much for the help.

[Nielsencu]

Just wanted to check if there's any suggestions regarding this issue, would be greatly appreciated! :)

[StoneT2000]

Will bump this up my TODO list today, thanks for the reminder

[StoneT2000]

For your URDF you will also need to send me the full folder, I am missing the mesh files

[Nielsencu]

Meshes: kortex_description.zip URDF: kinovagen3.zip

[UltronAI]

Hi @StoneT2000,

I'm working on a similar project and encountered the same issues—specifically, the arm motion being affected by changes in the gripper joints. I also struggled to apply the correct constraints to the Robotiq gripper.

I followed the tutorial linked in this comment and implemented debug/robotiq.py using the latest versions of SAPIEN 3. While some behaviors are a bit unusual, it seems mostly correct.

Screencast from 08-20-2024 06:11:51 PM.webm

However, when I used this approach to build a tabletop environment with the Robotiq gripper in ManiSkill 3, I encountered an unexpected issue. Given the success with SAPIEN alone, I'm unsure what’s causing the problem.

Screencast from 08-20-2024 06:17:51 PM.webm

To reproduce this issue, you can use the code in this fork.

mani_skill/examples/debug_robotiq_sapien.py contains my reimplementation of the tutorial code. Run it with python -m mani_skill.examples.debug_robotiq_sapien.py --model_constraint.

Running python -m mani_skill.examples.debug_robotiq_maniskill.py will reproduce my results in ManiSkill 3. The corresponding Robotiq agent is in mani_skill/agents/robots/robotiq/robotiq.py. Unfortunately, I cannot control the gripper correctly with either the PDJointPosMimicController or by directly changing its joint positions via GUI.

Thank you in advance for any advice on resolving this issue!

[StoneT2000]

Ok one issue is the old cvpr tutorial urdf definition is actually outdated/partly wrong. I am fixing it to make it work.

Also after discussing with @fbxiang, to simulate this very specific gripper well/accurately, it requires some complex drive creation code so mimic joints labels in the urdf cannot really be used. I'm hoping we can think of some general way to define complex grippers like this one via drives (which simulate better and are more stable).

[StoneT2000]

Fixed URDF files: https://github.com/haosulab/ManiSkill-Robotiq_2F/tree/v0.1.0 (there's a floating one and a non floating one, feel free to add these to your own robots). Also fixes the visual meshes so its not all gray.

@Nielsencu you should be able to look at the floating robot iq 2f 85 implementation and copy it over to your own custom robot with the kinova setup. Key thing to do is to add the correct passive/mimic controller configs and define _after_load_articulation function to setup the drives which make the gripper behave like real life.

@UltronAI You can try out the new floating gripper

python mani_skill/examples/demo_robot.py -r "floating_robotiq_2f_85_gripper" --zero-actions -c pd_joint_delta_pos

The drives are setup so only the outer knuckle joints actually control the gripper, the rest are passive. This will open the GUI and you can click the articulation, then the outer knuckle joint and set the position target to see it.