IK for franka - orientation problem

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!

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)

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?

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