Inaccurate robot arm motion control with deoxys

[ge79puv]

Hey there,

recently, I use deoxys to control the FR3 for the manipulation task. I utilize joint impedance controller in deoxys, and solve ik with ikpy. The pipeline of the execution is similar to that in ik_example.py in deoxys. However, I find that the eef pose after the movement of the robotic arm is not accurate, and there is always about 1 to 2 cm difference between the target eef pose and the current pose after movement. Could you tell me what’s the possible reason and what is the common practice to control the FR3 movement accurately. Following is the implementation of my code:

joint_traj = self.ik_trajectory_to_target_position(target_pose_robot, last_q.tolist(), num_points=100)
joint_traj = self.interpolate_dense_traj(joint_traj)
self.execute_ik_result(joint_traj, self.last_og_gripper_action)



def ik_trajectory_to_target_position(self, target_pose, start_joint_positions, num_points=100, verbose=True):        
    
    assert(len(start_joint_positions) == 7), "start_joint_positions should be a list of 7 elements"

    init_pose = np.zeros(15)      

    init_pose[5:12] = start_joint_positions         
    predicted_joints_seq = [init_pose]        

    last_q = self.robot_interface.last_q
    current_pose = np.zeros(15)      
    current_pose[5:12] = last_q
    current_pose = self.chain_left.forward_kinematics(current_pose)         
    current_pos = current_pose[:3, 3:]
    current_mat = current_pose[:3, :3]

    current_pos = current_pos.flatten()

    rot_steps = self.interpolate_rotations(current_mat, target_pose[:3, :3], num_points)   
    
    for i in trange(num_points):        

        pos = current_pos + (target_pose[:3, 3] - current_pos) * (i + 1) / (num_points)         
        target_pose_i = T.pose2mat((pos, T.mat2quat(rot_steps[i])))       

        ik_solution = self.chain_left.inverse_kinematics_frame(target_pose_i, initial_position=init_pose, orientation_mode="all")

        predicted_joints_seq.append(ik_solution)

    return predicted_joints_seq


def interpolate_dense_traj(self, joint_seq, minimal_displacement=0.005):        
    new_joint_seq = []
    for i in range(len(joint_seq) - 1):
        new_joint_seq = new_joint_seq + [joint_seq[i]]
        max_joint_displacement = np.max(np.abs(joint_seq[i] - joint_seq[i + 1]))
        if max_joint_displacement < minimal_displacement:
            continue
        else:
            num_points = int(max_joint_displacement / minimal_displacement)
            for k in range(num_points):
                new_joint = joint_seq[i] + (joint_seq[i + 1] - joint_seq[i]) * (k + 1) / (num_points + 1)
                new_joint_seq.append(new_joint)
    new_joint_seq.append(joint_seq[-1])
    print("increased joint sequence from {} to {}".format(len(joint_seq), len(new_joint_seq)))
    return new_joint_seq


def execute_ik_result(self, joint_traj, last_og_gripper_action):

    if last_og_gripper_action == 1.0:
        gripper_action = -1.0
    elif last_og_gripper_action == 0.0:
        gripper_action = 1.0

    for joint in joint_traj:            
        action = joint[5:12].tolist() + [gripper_action]            
        self.robot_interface.control(
            controller_type="JOINT_IMPEDANCE",
            action=action,
            controller_cfg=self.controller_cfg,
        )

        self.video_cache_step()