[Question] Difficulty Achieving Correct Orientation in 'PickCube-v0' with Pose Control

[shayan-ahmadi9]

Hello,

I'm currently working with the PickCube-v0 environment in ManiSkill2 (version 0.5.3) using the pd_ee_delta_pose control mode on Ubuntu 22.04.4 LTS. While the robot successfully reaches the desired position when controlling only the position, it struggles to achieve the correct orientation (rotation) when attempting to control the full pose. The issue is particularly evident when controlling both position and orientation (or even just orientation), as the robot fails to reach the target pose/rotation for most seeds other than env.reset(seed=1).

I'm using the following PDEEPoseControllerConfig settings:

frame = base use_delta = True normalize_action = True interpolate = True use_target = False

Could you please provide guidance on what might be going wrong with the orientation control and how I can effectively control the robot to achieve a specific target pose? Any suggestions or corrections would be greatly appreciated.

Thank you for your time and assistance!

import mani_skill2.envs
import numpy as np
import gymnasium as gym
from scipy.spatial.transform import Rotation
from sapien.core import Pose

# Initialize environment
env = gym.make("PickCube-v0", obs_mode="state_dict", control_mode="pd_ee_delta_pose", reward_mode="normalized_dense", render_mode="human")
state = env.reset(seed=2)

# Get the target and current poses in the robot's base frame
robot_pose = env.agent.robot.get_pose().inv()
target_pose_robot_frame = robot_pose * env.unwrapped.obj.get_pose()  # Target object pose
current_pose_robot_frame = robot_pose * env.agent.robot.get_links()[-3].get_pose()  # End-effector pose

# Adjust target pose to hover above the object
target_pose_robot_frame.p[2] += 0.05  
target_pos = target_pose_robot_frame.p
target_rot_quat = target_pose_robot_frame.q

current_pos = current_pose_robot_frame.p
current_rot_quat = current_pose_robot_frame.q

# Set thresholds and max deltas
threshold_pos = 0.01  # Position error threshold
threshold_rot = 0.05  # Rotation error threshold
max_delta_pos = 0.01  # Maximum position delta per step
max_delta_rot = 0.01  # Maximum rotation delta per step

def gripper_shortest_angle(angle):
    """Ensure angle is between -π/2 and π/2 for gripper rotation."""
    angle = angle % (2 * np.pi)
    if angle > np.pi:
        angle -= 2 * np.pi
    if angle > np.pi/2:
        angle = np.pi - angle
    elif angle < -np.pi/2:
        angle = -np.pi - angle
    return angle

def quaternion_to_gripper_shortest_euler(q1, q2):
    """Calculate the shortest Euler angle difference between two quaternions."""
    r1 = Rotation.from_quat(q1)
    r2 = Rotation.from_quat(q2)
    r_diff = r2 * r1.inv()
    euler_diff = r_diff.as_euler('xyz')
    euler_diff = np.array([gripper_shortest_angle(angle) for angle in euler_diff])
    return euler_diff

# Control loop to move towards the target pose
pos_error, rot_error = np.inf, np.inf
while (pos_error > threshold_pos) or (rot_error > threshold_rot):
    # Compute position and rotation differences
    pos_diff = np.clip(target_pos - current_pos, -max_delta_pos, max_delta_pos)
    rot_diff = quaternion_to_gripper_shortest_euler(current_rot_quat, target_rot_quat)
    rot_diff_norm = np.clip(rot_diff / (np.pi/2), -max_delta_rot, max_delta_rot)

    # Create the action vector with the gripper action set to 0.9 (open)
    action = np.concatenate((pos_diff, rot_diff_norm, np.array([0.9])), axis=0)

    # Take a step in the environment
    state = env.step(action)

    # Update the current pose from the new state
    current_pose_robot_frame = robot_pose * env.agent.robot.get_links()[-3].get_pose()
    current_pos = current_pose_robot_frame.p
    current_rot_quat = current_pose_robot_frame.q

    # Calculate errors
    pos_error = np.linalg.norm(current_pos - target_pos)
    rot_error = np.linalg.norm(quaternion_to_gripper_shortest_euler(current_rot_quat, target_rot_quat))

    env.render()

# Picking up the object by closing the gripper
grasping_steps = 50
for i in range(grasping_steps):
    # Move downward and then close the gripper
    action = np.array([0, 0, -0.05, 0, 0, 0, 0.9])
    if i == grasping_steps - 1:
        action[-1] = -0.5  # Close the gripper on the last step
    state = env.step(action)
    env.render()

# Lifting the object
lifting_steps = 50
for i in range(lifting_steps):
    action = np.array([0, 0, +0.05, 0, 0, 0, -0.5])  # Move upward with the gripper closed
    state = env.step(action)
    env.render()

# Close the environment
env.close()

[StoneT2000]

You can try turning off use_delta and set normalize_action False and interpolate false. if you keep taking a lot of the same action / target ee pose, it should eventually reach it

[StoneT2000]

@shayan-ahmadi9 has your issue been resolved?

[shayan-ahmadi9]

Thank you for your help!

I applied the recommended changes with the following configuration:

frame = base
use_delta = False
normalize_action = False
interpolate = False
use_target = False

I also adjusted the main control loop as follows:

# Control loop to move towards the target pose
pos_error, rot_error = np.inf, np.inf
while (pos_error > threshold_pos) or (rot_error > threshold_rot):

    # Convert Quaternion to Euler
    target_rot_euler = Rotation.from_quat(target_rot_quat).as_euler('xyz')

    # Create the action vector with the gripper action set to 0.9 (open)
    action = np.concatenate((target_pos, target_rot_euler, np.array([0.9])), axis=0)

    # Take a step in the environment
    state = env.step(action)

    # Update the current pose from the new state
    current_pose_robot_frame = robot_pose * env.agent.robot.get_links()[-3].get_pose()
    current_pos = current_pose_robot_frame.p
    current_rot_quat = current_pose_robot_frame.q

    # Calculate errors
    pos_error = np.linalg.norm(current_pos - target_pos)
    rot_error = np.linalg.norm(quaternion_to_gripper_shortest_euler(current_rot_quat, target_rot_quat))

    env.render()

The robot’s behavior has changed slightly, but it still struggles to reach the desired rotation. The issue persists, especially with achieving accurate orientation.

Any further suggestions or insights would be greatly appreciated!

Thanks again for your time and help.

[huang-fuxian]

I find use delta pose as action for EE mode in maniskill, can adjust the rotation nearly to the target one.