[Question] run_diff_ik.py can't work without reset the arm to the default pose

[nolann030628]

Question

I want to write an IK function basd on the tutorial, and I don't want the pos to go back to the default pose so I comment the robot.write_joint_state_to_sim(joint_pos, joint_vel), but the arm seems to can not compute the right joint_pos_des.

 if count % 150 == 0:
            # reset time
            count = 0
        # reset joint state
            joint_pos = robot.data.default_joint_pos.clone()
            joint_vel = robot.data.default_joint_vel.clone()
            # robot.write_joint_state_to_sim(joint_pos, joint_vel)
            robot.reset()

the code I commented is show above, I want to know how to solve this and use this ik controller in my custom env. I would be more than appreciated for any useful suggestions!

[RandomOakForest]

Thanks for posting this. What would be then the pose you expect the robot arm to be at? It seems write_joint_state_to_sim should be used. See the example in this tutorial. I will move this post to a discussion for the team to follow up.

[nolann030628]

In my understanding, write_joint_state_to_sim is to set the pos to the default pose when reset. But what if I don't what to reset and directly move to the next target? So I just comment this line, but the arm didn't reach the target frame as shown in the fig. My code is shown below, I think it should move to the goal pose, but it didn't.

 while simulation_app.is_running():
        goal_pose = torch.tensor([[0.5, 0, 0.5, 0.0, 1.0, 0.0, 0.0]], device=sim.device)
        if count %150 == 0:
            # reset time
            # count = 0
        # reset joint state
            joint_pos = robot.data.default_joint_pos.clone()
            joint_vel = robot.data.default_joint_vel.clone()
            # robot.write_joint_state_to_sim(joint_pos, joint_vel)
            robot.reset()
        ik_commands[:] = goal_pose
        diff_ik_controller.reset()
        diff_ik_controller.set_command(ik_commands)
     
        jacobian = robot.root_physx_view.get_jacobians()[:, ee_jacobi_idx, :, robot_entity_cfg.joint_ids]
        ee_pose_w = robot.data.body_state_w[:, robot_entity_cfg.body_ids[0], 0:7]
        root_pose_w = robot.data.root_state_w[:, 0:7]
        joint_pos = robot.data.joint_pos[:, robot_entity_cfg.joint_ids]

        # compute frame in root frame
        ee_pos_b, ee_quat_b = subtract_frame_transforms(
            root_pose_w[:, 0:3], root_pose_w[:, 3:7], ee_pose_w[:, 0:3], ee_pose_w[:, 3:7]
        )
        # compute the joint commands
        joint_pos_des = diff_ik_controller.compute(ee_pos_b, ee_quat_b, jacobian, joint_pos)
        print("joint pos des,", joint_pos_des)

        # apply actions
        robot.set_joint_position_target(joint_pos_des, joint_ids=robot_entity_cfg.joint_ids)

        # obtain quantities from simulation
        ee_pose_w = robot.data.body_state_w[:, robot_entity_cfg.body_ids[0], 0:7]
        
        # update marker positions
        goal_marker.visualize(goal_pose[:, 0:3] + scene.env_origins, goal_pose[:, 3:7].repeat(scene.num_envs,1))
        ee_marker.visualize(ee_pose_w[:, 0:3], ee_pose_w[:, 3:7])
        scene.write_data_to_sim()
        sim.step()
        count += 1
        # update buffers
        scene.update(sim_dt)