Question on Setting up Kinova3 to Work with CollisionWrapper?

[ariana-spalter]

I was setting up human-robot-gym and running through the demo scripts and had a question about getting the Kinova3 set up to work with the CollisionWrapper. I tested the base setup from human-robot-gym with the Schunk robot using the demo_reach_human_environment.py script and found there were no collisions when using the ExpertObsWrapper. And further relaxed this to when there was no expert and instead used the GymWrapper with action selection done through the following:

         action = env.action_space.sample()
         pos = np.array([env.sim.data.qpos[x] for x in env.robots[0]._ref_joint_pos_indexes])
         goal = env.desired_goal
         action[:pos.shape[0]] = np.clip(goal-pos, -0.5, 0.5)

and found the expected behavior of no collisions of any type.

I wanted to have the same outcome with the Kinova3 robot so I followed the same setup as the Schunk had by creating a Pinnochio Manipulator Model of the Kinova3 as is done for the Schunk in the human_robot_gym/models/robots/manipulators/schunk_robot.py file. However, I have been seeing all collision types occur when testing the Kinova3 in the demo_reach_human_environment.py script (both with the ExpertObsWrapper and without). Are there any extra steps I should be aware of in getting the collision wrapper working in this setup?

Right now, my file structure includes:

                     human_robot_gym/models/robots/manipulators/kinova3_robot.py
                     human_robot_gym/models/assets/robots/kinova3/robot.xml (from robosuite)
                     human_robot_gym/models/assets/robots/kinova3/robot.urdf (and the corresponding meshes folder)
                     human_robot_gym/models/robots/config/kinova3.json

For the urdf file I used the one from https://github.com/Leonardfjy/kinova_gen3_ros2/blob/humble-devel/src/kortex_description/arms/gen3/urdf/GEN3_URDF_V12.urdf and modified the joint limits to reflect the hardware limits of the robot and changed all joint types to revolute except for the last one (Actuator7) which I kept continuous so that Nb joints = 8 (nq=8, nv =7) when going through Pinnochio in the pinonocchio_manipulator_model.py script.

[JakobThumm]

In general, you can debug the CollisionWrapper using the visualize_pinocchio flag. If that visualization looks right, there might be some fine tuning of the parameters required for that particular robot. If you open up a pull request with your changes, I might be able to help.

[JakobThumm]

Your end effector behaves weird. You can see this if you activate the visualize_pinocchio flag. I would use that visualization to debug the self-collision feature. This might be the reason:

(Actuator7) which I kept continuous so that Nb joints = 8 (nq=8, nv =7) when going through Pinnochio in the pinonocchio_manipulator_model.py script.

The rest of the pinocchio model looks normal to me. Keep in mind that we currently only check the last point of the desired trajectory for collisions to keep the RL training at a reasonable speed. Therefore, some collisions might still occur but effects should be limited.

[ariana-spalter]

Thanks for taking a look! I did some edits on the urdf using the visualize_pinnochio flag and the model visually seemed fine to me but I am still seeing collisions in both the expert and non-expert demo scripts. Is there a specific type of collision I should be looking for that is expected vs. unexpected when debugging the behavior of the arm with the demo scripts or is it mostly based on how the arm visually looks within Pinnochio? The fixes I ended up doing was changing all the joints to revolute including making the end effector joint a revolute rather than fixed joint.

Also, I want to make sure I’m understanding the role of the CollisionWrapper correctly, what is the expected behavior of the CollisionWrapper without doing any RL training? The way I had been interpreting it was that it was a way to guarantee no static or self-collisions for all parts of the arm whenever a robot takes an action (in joint space or EEF space if the IK module is used too) even if no RL training occurred. Is a better way to think of the CollisionWrapper as a tool to help make the RL training converge to a policy with no static or self-collisions and on its own can be used to help reduce the amount of collisions occurring in an environment but not make any guarantees?

[JakobThumm]

Hi, with the Schunk robot we use a custom end-effector collision model that is slightly bigger than the end-effector. This reduces the number of collisions to almost zero. I did the same for the kinova3 and it seems to work as well. I didn't get any collisions in the first few tests. However, this method indeed does not give safety guarantees against self-collisions or static collisions. Only the safety shield gives guarantees for human safety. Why? Because we only check the last point of the trajectory for collision, not the entire trajectory. This could be implemented (and I can see if we can assign some time for that), but it would take significantly longer to check for collisions in the planned trajectory than right now. If you want to do RL training with this, you will get very annoyed since your training time significantly increases. Therefore the design decision to only check the last point of the trajectory. I hope that clarifies the question.

[ariana-spalter]

Yes, that clarifies things thank you! Now all of my demo scripts are working as expected with no collisions in my first round of tests.

I had another question in regard to the urdf model setup. I was able to get it to work well with your additions to the kinova3 urdf model but one thing I am still wondering is why the end effector joint had to be changed from “fixed” to “revolute” to make the PinocchioManipulatorModel setup work? Doesn’t changing the urdf file like that/adding a joint to the end effector effectively make it a different robot model than the base Kinova3 robot used in robosuite?

I was testing a rollout script with an agent I had from a previously trained policy in robosuite with the new Kinova3 model but am finding the size of the observation space is no longer matching if I switch to the Kinova3 using Pinnochio and was trying to figure out if there was a way to still have a Pinnochio Kinova3 model work with a previously trained policy with a Kinova3 or if I have to think of the two Kinova3 models differently. When I have tried to change the end effector joint back to a fixed joint it has had problems pop up with sizing in the pinnochio_manipulator_model.py (usually in the pin.forwardKinematics(...) within the update_configuration(...) function or on line 159 with T = self.pin_data.oMi[self.dof+ 1]).

[ariana-spalter]

I investigated a little more and the problem with the observation space difference between using the Kinova3 base model within robosuite and the Kinova3 Pinocchio model seems to be that in making the robot into a PinocchioManipulatorModel shrunk the size of gripper_qpos and gripper_qvel from 6 to 2 within the environment's observation space no matter which gripper is used (the base robosuite gripper like robotiq85 or a robotiq85 gripper formated the same way as rethink_valid_gripper.py). I don't have a clear idea as to why this might happen, what is the reason I would be seeing that happen and is there a way to change it back to a size 6 value for both the gripper_qpos and gripper_qvel?

[JakobThumm]

You are using our custom gripper model here https://github.com/ariana-spalter/human-robot-gym/blob/2de2c2dab93b4182503de72d67a6091977c2bde9/human_robot_gym/models/robots/manipulators/kinova3_robot.py#L14 The custom gripper simplifies the gripper structure see https://github.com/TUMcps/human-robot-gym/blob/mirror/human_robot_gym/models/grippers/rethink_valid_gripper.py Therefore, there are only two gripper joints left. In our trainings, we use the gripper aperture instead of the 6 gripper joint positions. This simplifies the observation space. https://github.com/ariana-spalter/human-robot-gym/blob/2de2c2dab93b4182503de72d67a6091977c2bde9/human_robot_gym/environments/manipulation/human_env.py#L1509 To test your pre-trained models

1. make sure that you adept the setup_observables() function in the environments, e.g., here we deactivate the gripper joint pos observable: https://github.com/ariana-spalter/human-robot-gym/blob/2de2c2dab93b4182503de72d67a6091977c2bde9/human_robot_gym/environments/manipulation/reach_human_env.py#L638 I guess you want to have the gripper joint pos and vel active and the aperture inactive.
2. use the robosuite grippers instead of the custom human-robot gym ones.

[ariana-spalter]

Thank you for the clarification! I am now able to test my pre-trained models using your recommendations.