[Polymetis] Joint Locking Issues

[AlexanderKhazatsky]

Type of Issue

When doing joint control (both qpos control and qvel control) the joints frequently freeze because a joint position or joint velocity limit was exceeded.

Description

I think it's great that Polymetis has checks for this, but they way the code handles it is extremely irritating.

For example, when I am doing everything via joint position control, rather than having the joint impedance controller clamp the outputted joint velocities and torques to a safe limit, they send them out, and then the safety controller freezes the whole robot because a safety constraint was violated. And, because I have no access to the outputted velocity/torque values, I can't clamp them myself.

Expected Behavior

I think it would be much more helpful if the safety controller just made the robot move slower and softer, and didn't force it to freeze up for a few seconds.

Steps to reproduce

Lmk if this is needed.

[AlexanderKhazatsky]

On a similar note, I'm working on an IK solver that tries to work around these issues. It is backed by dm_robotics, so it could be of interest to the dev's

Although it can avoid the joint limit errors, I can't do anything about the velocity and torque errors since it's the low level controller outputting those

real_robot_ik.zip

[exhaustin]

Good point, thanks for the feedback!
For now, a quick fix would be to limit your pose updates to be within a proximity of the current pose.

If that doesn't work, you should be able to clamp them yourself by writing a custom controller. A similar example can be found here, and you can also copy / inherit from our current joint impedance controller here

[AlexanderKhazatsky]

Hey Austin! I tried out the new controller, and it is SO MUCH better! Thank you so much for all your hard work!!!

The only major remaining roadblock I have in using this controller for large scale demo collection is the joint locking issue. I think I've found a solution to half of it (wanted to run it by you since you're an awesome roboticist), and would love your help in solving the other half.

I believe there are two causes of joint locks:

1. Hitting a joint limit and using updates of the form desired_new_pos = desired_current_pos + action. Once you violate a joint limit, its very hard to un-violate it with such an update rule. I believe I've fixed this issue by projecting the desired_new_pose into a feasible space, and using that instead in the next update rule (it could be worthwhile for Polymetis to implement something like this under the hood for a delta={True,False} argument):

EXAMPLE

def update_desired_ee_pose_local(
    self,
    position: torch.Tensor = None,
    orientation: torch.Tensor = None,
) -> int:
    """Update the desired EE pose used by the Cartesian position control mode.
    Requires starting a Cartesian impedance controller with `start_cartesian_impedance` beforehand.
    """
    joint_pos_current = self.get_joint_positions()
    ee_pos_current, ee_quat_current = self.get_ee_pose()
    ee_pos_desired = ee_pos_current if position is None else position
    ee_quat_desired = ee_quat_current if orientation is None else orientation

    joint_pos_desired, success = self.solve_inverse_kinematics(
        ee_pos_desired, ee_quat_desired, joint_pos_current
    )

    if success:
        feasible_pos, feasible_quat = self.robot_model.forward_kinematics(joint_pos_desired)
    else:
        log.warning(
            "Unable to find valid joint target. Skipping update_desired_ee_pose command..."
        )
        feasible_pos, feasible_quat = self.robot_model.forward_kinematics(joint_pos_current)

    self.update_desired_joint_positions(joint_pos_desired)

    return feasible_pos, feasible_quat

Then Later: desired_current_pos = [feasible_pos, feasible_quat]

EXAMPLE

2. As discussed above, the outputted joint velocities and positions can break Polymetis's safety constraints. We can fix this by clamping the outputted low level joint velocities and positions (however, I can't figure out where I should be doing this in Polymetis), and then (similar to above) returning the feasible_pose after clamping. Another option which is probably harder is if there was some robot_interface function that allowed you to pass in desired commands (joint or cartesian) and then returned a feasible_and_safe version of it.

Looking forward to your reply :)

[AlexanderKhazatsky]

Some other small issue that could be good to inform you of:

• I think the IK could definitely be more intelligent. I can try using one I prepared above and let you know how it goes.
• The new controller oscillations around certain desired poses rather than converging. I've included an example video:

IMG_4884.MOV

[exhaustin]

Hi Alexander! Glad you liked our new controllers!

Joint locks & limits

I completely understand that constantly triggering safety constraints out of nowhere can be frustrating, but we also don’t want to modify user commands too much behind the curtains either. We will be iterating on what the ideal user interfaces should be, which will take some time, but you are helping us to do so immensely by sharing your detailed experiences in these issues!

Projecting the commanded position into a feasible space might be desirable and a handy functionality to have, but as the feasible space is non-convex I don’t know how to do it off the top of my head. Also I’m not sure I understand what your script is trying to do. It seems like you are simply using the current pose if the target pose is not feasible.

If the action space of your policy is delta positions & orientations, I would simply constrain the action space to a reasonable magnitude (ex: no more than 10cm and 5 degrees per second, so like 5mm if you’re controlling at 50hz). This should be enough to not trigger any velocity & torque constraints. Combine that with desired_pose = current_pose if unfeasible else desired_pose to prevent hitting joint limits and you should be good.

IK

Our IK implementation currently is indeed quite naive, implemented as simply a gradient descent on forward kinematics. We will be focusing on improving on that aspect next, and will also consider the implementation in dm_control. The line of work will be tracked in #1336.

Oscillations

The controller oscillations is concerning, can you share the joint positions at which this is happening so I can try to repro? However, it does look like you are controlling the arm at a rather awkward pose near certain joint limits, and thus moving the manipulated objects farther away from the robot would definitely help.

[AlexanderKhazatsky]

My proposed solution:

• My logic here was to project the pose into what's feasible joint wise. However, on closer inspection, I think the real benefit here is coming from using feasible_pose instead of desired_pose when an IK solution is not found (hence keeping the process in feasible space)

Action Velocity is not the only (or main) cause of joint locking:

• I have tried changing the norm of my actions to be unreasonably small, but the issues I'm experiencing are still just as present. I've included an example video below.
• I have one possible theory: Maybe when a nearby solution is not found, the IK finds a solution that is very far in joint space and that abrupt commanded movement is the source of these issues?
• Something of interesting note. I remember finding that the old cartesian controller (even though it was inaccurate) did not have this issue. I could be wrong though. But if im not, maybe something specific to IK approach is causing it?

Uploading small_video.mov…

Oscillations:

• After some investigation, I think this might have been the fault of my stack (action norms too big?). I'll update you on if I see this issue again :)

[mohanksriram]

We're also facing similar joint limit reached issues in our setup. It would be really helpful if we can identify the root cause of the problem.

[exhaustin]

Which functionalities are you using that are causing the joint limit issue?
If the error specifies that joint positions are reaching the limits, then the root cause should be quite straightforward -- the joints are running into their limits (the limits we impose within Polymetis is only a few degrees stricter than the hardware limits, which can be found here).

If you are using move_to_joint_positions or update_desired_joint_positions, then you are probably commanding it to joint positions that exceed the limits.
If you are using move_to_ee_pose or update_desired_ee_pose to command the robot, then it might be due to limitations of our current IK planner, or due to the fact that you are commanding it to an infeasible pose.

[AlexanderKhazatsky]

Hello all, I hope you've been well! We have begun our data collection process, and the #1 comment back from our beta testers was complaints about the joint locking issues. A solution of any kind to this problem (even a short term solution) would be extremely appreciated :)

• One idea is a function which tells us is_ee_pose_feasible(pos, quat), which tells us if it's reachable / won't lock joints if we send it. This would be enough for now :)

Also, do there happen to be any updates on progress for this?

[1heart]

@AlexanderKhazatsky @mohanksriram

1. Can you specify what limits are you hitting? Is it joint position, joint velocity, joint torques, Cartesian soft limits, Cartesian hard limits, self-collision? This should be a fairly comprehensive list; please paste the server logs when you encounter the joint-locking behavior.
2. How are you generating the desired joint positions/velocities? From your video, it does look like you're asking for pretty pretty awkward joint configurations.
   a. If you ask for infeasible joint states, the hardware itself (i.e. libfranka) refuses to continue; Polymetis may "lock up" by attempting to reinitiate the control loop, but there's basically nothing we can do if you request infeasible joint states. You can customize the limits if you want, but they're pretty close to the hardware-defined limits.
   b. You can run arbitrary torch policies with Polymetis, so it's very simple to add a clamping function at any step you want. Read the TorchControl docs for the details; if you have controllers that work better, we're very happy to take contributions!
   c. In particular, if you have a precise function definition of "feasible ee pose", please do share the code. We currently have soft limits which push the end-effector when it nears the edges of the workspace (defined here), but we aren't doing any self-collision checking or anything more complicated. Our default joint-space planners take into account velocity limits by computing a reasonable time_to_go such that we avoid violating velocity limits. If you have a better definition of feasible, please share!

[1heart]

In particular:

1. If you want to clamp joint torques, that's just the output of the forward function for every controller (e.g. HybridImpedanceControl).
2. If you're using the output of a joint-space planner, you have direct access to the desired trajectory (e.g. here).
3. If you're using the update_desired_*_positions paradigm, you can clip the desired values before sending them to the update function.

[AlexanderKhazatsky]

1. I am hitting joint position limits (probably because IK uses real joint limits instead of the padded joint limits. This seems like a simple fix. I am also (this is the hardest to solve) hitting joint velocity limits. This happens with no correlation to actual commanded speed (ie. it occurs even with minute delta movements). Maybe the IK planner commands large joint changes under the hood? Typically the violations are by a very small amount. If it would still be helpful to post the logger outputs I'm happy to do this.
   2a. I slightly reduced the occurrence by clamping there desired commanded angles between -pi/2 and pi/2 (which I think should be a reasonable request). However, the issues are still very much present. I would like a way to operate within this cartesian space without running into joint locking issues. If a certain pose would cause issue, I would just like the command to return a message me telling me so, and this will fix my problem.
   2b. Clamping torque output helped a little bit, but didnt solve the issue entirely, while also making the robot unreasonably slow :(
   2c. When I request a "feasible ee pose", I am referring specifically to a use case for the cartesian impedance controller, which is where I encounter these joint locking issues. I would really appreciate some function "feasible ee pose" which just tells me whether or not sending a certain cartesian impedance command will cause a joint lock. An equivalent function for sending a joint impedance commands would also be fine. Then I can just not take this step and my problem will be solved :)

In response to your second comment, the issue still happens when clipping desired values. Are you guys encountering it on your side?