Orientation Control Not Acting As Expected

[AlexanderKhazatsky]

Type of Issue

Select the type of issue:

• [x] Bug report (to report a bug)

Description

While position control has been quite nice, orientation control seems to have a lot of issues on my end.

1) Only one of the DoF works as expected (left and right rotation of the gripper). The other two seem to be on some weird axis that I've never seen before. 2) The orientation matching is generally much poorer than the position matching. Ex. Moving one axis one direction for x time steps, then backwards for x time steps results in a significantly different position. 3) The gears get start to jitter when one orientation is maxed out. This seems undesirable.

Steps to reproduce

I made a small adjustment to the impedance controller script and have attached it below.

https://user-images.githubusercontent.com/30299984/173477316-89d2ed10-1f12-4065-bc5e-7cdfbae15dac.mov

https://user-images.githubusercontent.com/30299984/173477501-71f7ea73-16d9-4918-a27f-a84016b4a066.mov

# Copyright (c) Facebook, Inc. and its affiliates.

# This source code is licensed under the MIT license found in the
# LICENSE file in the root directory of this source tree.
import torch
import time
import numpy as np
from polymetis import RobotInterface
from scipy.spatial.transform import Rotation

def euler_to_quat(angle):
    rot = Rotation.from_euler('xyz', angle)
    quat = rot.as_quat()
    return quat

def add_quat(quat_1, quat_2):
    rot_1 = Rotation.from_quat(quat_1)
    rot_2 = Rotation.from_quat(quat_2)
    quat = (rot_1 * rot_2).as_quat()
    return quat

if __name__ == "__main__":
    # Initialize robot interface
    robot = RobotInterface(
        ip_address="localhost",
    )

    # Reset
    robot.go_home()

    # Cartesian impedance control
    print("Performing Cartesian impedance control...")
    ee_pos, ee_quat = robot.get_ee_pose()
    ee_pos, ee_quat = ee_pos.numpy(), ee_quat.numpy()

    robot.start_cartesian_impedance()

    pos_action = np.array([0, 0.0, 0.0])
    deg_action = np.array([0.0, 0, np.pi])

    for i in range(100):
        if i == 50:
            pos_action = -pos_action
            deg_action = -deg_action

        ee_pos += pos_action / 100
        ee_quat = add_quat(ee_quat, euler_to_quat(deg_action / 100))

        robot.update_desired_ee_pose(
            position=torch.Tensor(ee_pos),
            orientation=torch.Tensor(ee_quat))

        time.sleep(0.1)

    robot.terminate_current_policy()

[exhaustin]

1. The x and y axis for the Franka end-effector is rotated 45 degrees around the z axis. This is defined within the official URDF file from Franka Emika, which you can verify by using forward kinematics in the robot model.
2. You should not be hitting the limits at all times. We implement "safety controllers" which push the robot away from the edge of the state space, but changing the behavior of controllers in doing so. Thus it is normal to experience undefined behavior depending on what controller you're running. You can disable the safety controllers here, but then the robot will simply error out on the client side when running into the limits, which you will need to deal with yourself.

p.s. Thanks for providing the videos in this issue, they were very informative and made it very clear what behaviors you are experiencing.

[AlexanderKhazatsky]

Ah very interesting… Given that the standard axes are probably significantly more useful for researchers, it would be extremely helpful if you guys could include a Boolean for using the traditional orientation axes instead :)

As for the safety controller, that makes sense!

[exhaustin]

The "standard" axes are the ones provided by Franka, which happen to be visually off by 45 degrees from the symmetries of the Franka Hand shape. In fact, the axes will be aligned when connecting the Franka Panda to other grippers such as a Robotiq 2F (see image below).

The interface we provide stays true to the URDF and is the same as vanilla libfranka in terms of coordinate frames. We do not plan to change that within Polymetis to avoid confusion and unnecessary complexity. It shouldn't be hard to wrap whatever coordinate transform you see fit outside of Polymetis, though.

Closing the issue since there is nothing to be fixed.

[suraj-nair-1]

Hello, I had a quick follow up question regarding this.

@exhaustin - If I'm understanding correctly, you are saying the coordinate axis used for translation is different than the one used for orientation?

Also I didn't realize this was the default in the Franka URDF. Would you happen to have the link to the URDF? In the libfranka/franka_ros cartesian impedance controller, the coordinate axis for translation and rotation are aligned, and the rotation axis is visually aligned with the symmetry of the Franka Hand shape.

[exhaustin]

No, I'm just saying that the frame for the Franka Hand is officially defined to be at an angle to its geometric features. As you can see in this diagram, X_F is neither parallel nor perpendicular to the protruding handle.

I'm not sure about the situation in the libfranka/franka_ros cartesian impedance example, but the translation and rotation always correspond to the transformation from the base frame to the end-effector frame. I don't think there is such thing as misalignment between the translation and rotation axes in any case.

Could it be possible that in the libfranka/franka_ros cartesian impedance example, the robot happens to be at a pose where the visual features of the Franka Hand is aligned with the frame of robot base? For example, with the flat face of the Franka Hand facing forward, it is true that trying to rotate the "rotation coordinate" of the end-effector around X would simply rotate the face clockwise or counterclockwise.

[suraj-nair-1]

Got it I see, that makes sense. I think our confusion was because we were naively rotating 45 deg along the z to "straighten" to the gripper face, but as you pointed out that also moved the x and y axis. You explanation makes sense, thanks for the detailed answer!

For franka_ros I think the setup you describe is right, which explains why that behaves a bit differently.

In any case, we plan to use the robotiq gripper, in which case I think everything will be pretty intuitive. thanks for the help!