IK for franka - orientation problem

Autonomous Machines Robotics - Isaac Isaac Gym
1

Hi! I’ve tried to replicate the franka_cube_ik example to better understand how it works. My idea is to create a function where I pass an action [x,y,z,g], where x,y,z is the movement along those axes w.r.t. the end-effector, and g stands for gripper value control. If someone did this already, I would be very grateful if it could be shared here.

Now onto the problem! After implementing the function mentioned above I’ve tested a couple of combinations for axis movements. I’ve also added a fixed orientation for the end-effector. In other words, I want the robot hand to always be perpendicular with the surface and facing forward. Here’s the snippet for that:

o = gymapi.Quat.from_axis_angle(gymapi.Vec3(0,1,0), np.radians(-180.0))
goal_rot = torch.tensor([[o.x, o.y, o.z, o.w]] * num_envs).to('cuda:0')

The problem is that this orientation is no longer maintained when I add a movement in the z-axis (vertical). However, it works perfectly when I only move on x or y axis. Please let me know if you need more details!

When analysing the franka_cube_ik example, I’ve noticed a weird movement that I think it’s related to this problem. When lifting the cube, the hand stays at an angle instead of straightening. I don’t know if that’s intended, but it looked similar to my problem.

Thanks in advance!

2

Turns out the problem lays in setting the initial dof states and initial position targets. After adding the following, the problem dissapeared:

# set initial dof states
gym.set_actor_dof_states(env_ptr, franka_actor, default_dof_state, gymapi.STATE_ALL)

# set initial position targets
gym.set_actor_dof_position_targets(env_ptr, franka_actor, default_dof_pos)
3

That’s great, @mihai.anca13 !

The only question I have - it looks like you are using the older, non-tensor API, while in the original franka ik example tensor API was used: self.gym.set_dof_state_tensor...(). Is it true?

4

Good catch, @vmakoviychuk !

In /rlgpu/tasks/franka.py you are absolutely right:

    self.gym.set_dof_position_target_tensor_indexed(self.sim,
                                                    gymtorch.unwrap_tensor(self.franka_dof_targets),
                                                    gymtorch.unwrap_tensor(multi_env_ids_int32), len(multi_env_ids_int32))

    self.gym.set_dof_state_tensor_indexed(self.sim,
                                          gymtorch.unwrap_tensor(self.dof_state),
                                          gymtorch.unwrap_tensor(multi_env_ids_int32), len(multi_env_ids_int32))

However, I was following /examples/franka_cube_ik.py, where the code looks like this:

# set initial dof states
gym.set_actor_dof_states(env, franka_handle, default_dof_state, gymapi.STATE_ALL)

# set initial position targets
gym.set_actor_dof_position_targets(env, franka_handle, default_dof_pos)

I will adjust my code to use the tensor version.

Can you please explain what self.global_indices stores in franka.py? I don’t understand why 2 ids are stored for each robot ([env_ids, :2]).

Thanks in advance!

2 Likes