[Polymetis] General Cartesian Control Issues

[AlexanderKhazatsky]

Type of Issue

Select the type of issue:

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

Description

Given how clean and well maintained this repo is (thank you!); I’d really like to use it for a large data collection project which requires 6 dof control through oculus based teleoperation.

However, I’ve generally been struggling to use polymetis for Demo collection through it’s Cartesian control features. For these experiments I’ve been using the Cartesian impedance controller.

To formalize what I’m seeing, I’ve plotted the desired trajectory (black) versus the achieved trajectory (red). The robot gets the desired pose every 1/hz seconds (in this case, hz is 20). I’m happy to share the code for this experiment if it would be helpful.

As you can see in the plots below, position mapping is pretty sub par, and orientation matching is very troublesome. I have noticed that switching from an impedance controller to set_ee_pose makes orientation control much more accurate, but not position control.

On a side note, there is significant drooping when doing delta control (Ie. next_pos = curr_pos + action, even with the gravity compensation on). I believe the issue has been even worse since I’ve switched to a robotiq gripper. It would be really cool if the controller accounting for which gripper is being used.

Things I’ve tried

Tuning the gains helps, but at a huge cost to smoothness. I can see that maybe this is an inherent problem, but I’ve discussed the issue with other labs who use alternative controllers and it is significantly better on their end (granted, a lot messier!).

Switching from a Cartesian impedance controller to using set_ee_pose helps (mostly just with orientation matching), but it is extremely jerky in movement.

For the drooping behavior, using next_pos = desired_curr_pos + action instead helps, but creates other issues.

Checklist

• Significant arm dropping (with default and robotiq gripper)
• Position and orientation matching is poor

[1heart]

• There are definitely some orientation tracking issues in the default controllers right now. The hopefully-nice thing about the Polymetis design is that you can hotswap any (sufficiently performant) PyTorch policy. If possible, would you mind pointing to the implementations better-tracking controllers? You can try implementing them as Torch policies, and we'd be happy to also try them out / potentially merge them in as defaults.
• For gripper drooping -- just a quick check to make sure you updated the end-effector inertia parameters in Franka Desk.

[exhaustin]

Hi @AlexanderKhazatsky! Just wanted to add that the Cartesian Impedance controller we provide is a local feedback controller that might lead to awkward joint poses (thus affecting accuracy) when executing large trajectories. An IK planner would be ideal for these kind of situations. We plan to add a decent default IK planner to the RobotInterface, but you might need to implement your own IK and use joint position control for these trajectories for now.

Regarding the drooping, updating the end-effector parameters as @1heart mentioned would definitely help, but you should still control it with "next_pos = desired_curr_pos + action" if possible. As long as there is even a tiny bit of error in the tracking, they WILL accumulate over time (and most likely droop) if your control scheme is "next_pos = curr_pos + action". What "other issues" are you getting when you do "next_pos = desired_curr_pos + action"?

[AlexanderKhazatsky]

Firstly, I just wanna say that you guys are the best - thank you so much for consistency responding both promptly and thoroughly.

Other Controllers: It looks like one of them uses the built in franka impedance controller, another (which I unfortunately am not allowed to share) uses libfranka + zmq + protobuf, and another one is FrankaPy.

EE Inertia Parameters: I didn't know about this, thanks I'll try this!

IK Planner: I see, thank you for the info! Out of curiosity, what's the timeline on releasing this controller? Furthermore, do you have any IK for franka that you recommend? I saw there was one built into Polymetis, but I wasn't sure how to make it take current joint positions into account. Without this, it seems to be outputting joint configurations that are not nearby.

Delta Control Issues: When controlling with "next_pos = desired_curr_pos + action", the robot tends to get stuck around the edges of the control space. I think this is because it begins requesting unreachable positions. However, this can probably be fixed by clamping next_pos within the limits as defined by the robot. Additionally, I found this control format to get jammed much more frequently (probably for a similar reason of "current_pos" drifted into an invalid zone, then not being able to return to valid state spaces).

General Comment: For robot learning research there’s a large demand for robot controllers that can be updated in the manner described above. For example pose control with small pose changes at a frequency of ~10 hz (with impedance to avoid breaking things). If you guys could make a controller specifically for this framework (which I believe it sounds like you're doing?), it would be a lifesaver!

[AlexanderKhazatsky]

On another note, it might be helpful to have a function like get_desired_ee_pose which returns the desired pos clamped into state space or something. Ie. Just a func which you could use for next_pos = desired_curr_pos + action that doesn’t cause the issues. Or are the issues not happening on your side when you go outside boundaries?

[exhaustin]

Thank you for your kind words!

• Unfortunately we don't have a timeline for a built-in IK planner yet. The inverse kinematics implementation we currently include in Polymetis only solves for a joint pose given a target end effector pose (instead of a full trajectory), the API of which is documented here. Not sure how much this will help, but by specifying rest_pose as the current pose, the result should be as close as possible to the current joint pose within the null space, while achieving the desired EE pose.

• I don't think I understand why the controller will request unreachable positions when given "next_pos = desired_curr_pos + action". We have no issues (either getting stuck or "jamming") while executing small motions in this exact manner when using our teleoperation stack.

• We're not "clamping" the desired pos within the Cartesian impedance controller at all. We only have safety controllers at the robot client side that prevents the EE from going outside a certain bounding box by applying forces. However, I agree that it will be helpful to include certain internal controller states (such as desired pose) within the returned state. No timeline can be promised as of now, but I made an issue to keep track of it: #1285.

• We will be continuously trying to improve the "move to" and "impedance control" methods, and I believe it should require very minimal wrapping from the user to go from there to the delta position control method you're using right now. Thus, I don't see a pressing need to provide something less general like that natively, but I wouldn't mind to be convinced otherwise.

[1heart]

@AlexanderKhazatsky to be clear, our Cartesian impedance controller is reimplemented directly from the built-in libfranka impedance controller iirc (specifically, this file). It's possible there's an error on our end, or there's some other difference -- can we pin down exactly what the diff between running libfranka vs. ours?

Happy to hop on a call to debug these issues if that'd be faster.

[AlexanderKhazatsky]

Using polymetis IK: I did this yesterday and had some success with it! However, I don’t think it’s designed for consecutive calls, because it frequently moves to joint limits unnecessarily. I imagine there are IK solvers which try to keep joints near center to avoid hitting locks. Do you happen to have any recommended solvers? I’ve heard good things about dm_control IK.

Controller requesting unreachable positions: Imagine I keep moving the robot forward. Eventually curr_pos will be like [10000, 0, 0]. At least on my end sending such requests locks up the robot. On a separate note, even when doing joint control using polymetis IK (which I imagine doesn’t return invalid joint configurations, this happens (it would be great if there was a more elegant solution. As this usually forces me to close down the entire program):

https://user-images.githubusercontent.com/30299984/179118993-993c2e1c-76b6-4d71-8132-67c02b4b52dd.MOV

Internal Controller States: Thank you!!

Delta Controllers: I completely agree that minimal wrapping is required to do this. I think my main reason for asking was so that the repo has a well maintained, tested, robust, and smooth controller for settings like the one I would like to use polymetis for (Ie. Things like live demo collection, or running RL agents. Basically things were actions are decided live during the trajectory). This is something that is currently unavailable and pretty frustrating to build from scratch, and I alone know like 5 labs who would jump at the chance to use polymetis if they supported such a controller out of the box :)

Figuring out the difference between controllers: I can set up the old controller I had that used the built in functions for a fair comparison, confirm the issue with the same codebase (where only polymetis vs libfranka functions are swapped), and get back to you!

[exhaustin]

Re: IK - We've been using trak-ik with moderate success in some experiments, but it is also not well designed for continuous motions.

Re: Controller requesting unreachable positions - You should be able to define your own state space by clamping the desired positions manually before sending them to Polymetis, which should not have that much of a difference from running into the lower level EE position constraints. We don't provide this natively because the desired range of motion varies between different experiment setups, and again would require very minimal wrapping from the user to do. Again I stress that getting rid of the drooping will be physically impossible if you are running "next_pos = curr_pos + action", even in simulation.

Re: Standardizing delta controllers - I think what we're providing with start_cartesian_impedance is already a standardized way of stabilizing around an EE pose, and the delta controller you want should literally be as simple as updating the desired pose value. I would say that the main issue with the status quo is the lack of IK planning which leads to awkward joint positions and thus inaccurate control, which we will be focusing our efforts in this workstream on.

[AlexanderKhazatsky]

Controller requesting unreachable positions: Ah, it seems that my issue was that when I wasn’t clamping positions, I’d request things super far away, which would cause safety limit violations, which was triggering automatic error recovery. Clamping solved most of my issue, but I still get occasional safety limit violations which freeze the robot. It would be nice if there was a way to check if an action would be safe, or clamp it to the nearest safe action. I can look into this on my own for now though!

Cartesian Impedance: Yes, I fully agree that the Cartesian impedance controller would be perfect for this type of use case, and that was my original hope.

Currently the Cartesian impedance controller shows promise for sure (smooth xyz control, although arguably could be more accurate). However, its faulty orientation matching is practically unusable (the performance difference between this and something like set_ee_pose suggests that surely there must be some type of bug or bad parameter).

Let me know if it would be helpful to share the code for the above plots. I’m currently blocked by this issue, so I’d be happy to help fix it :)

[1heart]

We actually have some "soft" safety controllers which push the end-effector within a predefined volume, in addition to the "hard" limits. They have tunable parameters if that helps.

Yes, would you mind sharing the code? That would be helpful. In addition, if you have a particular controller you prefer, you can just directly implement it in Torch and try it out, or modify the current Cartesian impedance controller.

[AlexanderKhazatsky]

Sounds good! Also, I realized that I only get this joint locking issue when doing IK + joint impedance, and not when I do cartesian impedance directly. So I wonder if there is some safety constrains I have to watch out for that is usually taken care of under the hood with cartesian impedance control.

Here's the script. Let me know if you have any questions!

Traj_Follower_Plotter.zip

[exhaustin]

I am currently trying to improve the current EE tracking as we are also experiencing similar issues internally.

However, I took a quick look at your script and noticed that you are calling start_cartesian_impedance and start_joint_impedance every timestep, which is not necessary and will slow down your timesteps since you are sleeping for a fixed amount of time. Basically start_X_impedance sends a controller to Polymetis, and update_desired_X updates the desired states for that controller once it's running in Polymetis. (Sorry the documentation is not there yet, which will be worked on once the feature set is more stable)

[AlexanderKhazatsky]

The reason for those lines are that the impedance control dies periodically. When there is already an impedance controller running it causes an error promptly (which the try except block catches). In practice this has been a decent solution, but lmk if you have a better one!

[exhaustin]

try this:

try:
    robot.update_desired_X(...)
except grpc.RpcError:
    robot.start_cartesian_impedance()
    robot.update_desired_X(...)

[AlexanderKhazatsky]

Building off of this, I've had a great deal of success with the dm_robotics 6 dof control procedure in simulation, and I think it could be worthwhile for us to set this up together for Polymetis:

https://github.com/deepmind/dm_robotics/blob/48d5f0bfd76ad497faabd5823a25d89d4526b92e/py/moma/effectors/cartesian_6d_velocity_effector.py

Or alternatively, something like this:

https://github.com/deepmind/dm_control/blob/main/dm_control/utils/inverse_kinematics.py

[AlexanderKhazatsky]

One thought, could it partially be due to the default gain values on pose matching? I think they are currently [150, 150, 150, 10, 10, 10] (aka putting much more weight on position matching than orientation matching).

I think results seem to be better when mapping pose control to joint positions through Ik than using the given gain values for joint control. But, doing this (at least through CLIK, need to check if it happens with resolved rates control) creates the issue of locking joints. Maybe because the CLIK IK uses the URDF joint limits rather than the limits defined in the safety controller?

[exhaustin]

Sorry for the late response, but I'm working on some exciting changes that will bring the end-effector tracking error down to less than 5mm and 2 degrees for position and orientation respectively. Will update this issue once the PRs are merged.

The translational gains and rotational gains operate on different units and thus differ drastically in scales. In fact, the value of 10 for the rotational gain already feels a lot stiffer than 150 for the translational gain. If you want to get into the math, 1cm (0.01m) positional error 150 gain = 1.5 N force, while an visually similar 3 degrees (0.05 rad) of orientation error 10 gain = 0.5 N*m torque. That's 5 N force at the end of a 10cm (0.1m) long gripper.

The features I'm working on right now involves using IK to improve the position/orientation matching, so will definitely also look into dm_control and see if it improves upon our current implementation. Thanks!

[AlexanderKhazatsky]

Can’t tell you how happy I am to hear this! Do you have an estimate of when I can expect this :)

As for the IK, I posted a wrapper that abstracts away most of the troubles in another issue. Hopefully that saves you some time in trying it!

On another note, I found a lab with high quality orientation control on the franka, and figured I’d share their tips and tricks. All credit goes to Minho Heo.

“ Hi Sasha,

The impedance controller offered by Polymetis is not backed by OSC. You can take a look at the default controller they offer here https://github.com/facebookresearch/fairo/blob/main/polymetis/polymetis/python/torchcontrol/policies/impedance.py I would recommend first trying the default controller they offer and see if the quality of the controller's rotation matches your requirements. In our case, since we needed complex manipulation, the controllers they offered were not enough, so we ported OSC from perls2.

If you want to port perls2 to Polymetis, the procedure you have to take is

• You have to modify perls2 controller to follow torchcontrol interface like this https://github.com/facebookresearch/fairo/blob/main/polymetis/polymetis/python/torchcontrol/policies/impedance.py#L76
  • Basically define "forward" method (def forward(self, state_dict: Dict[str, torch.Tensor]) -> Dict[str, torch.Tensor]:), and compute joint torques here.
• You have to modify/annotate all the code of your controller (and the method they are calling) to use Torch tensor, not Numpy array (which perls2 is using). This is because Polymetis JIT compiles everything and sends it to the real-time-capable workstation connected to the robot.

Note that the OSC controller requires more heavy computation, so you might encounter "communication_constraints_violation" errors. This is because your workstation could not compute torques using OSC in the robot's control loop interval (which is 1000Hz). To handle this, we used some tricks. We repeated the same joint torques computed by OSC N times (our case 4) and only computed torques every N step. Also, you might have to use a "good enough" CPU for a real-time-capable workstation (more than 4.6 GHz CPU speed).

If you have more questions, please feel free to ask! Best, Minho Heo”

[exhaustin]

I can't promise anything but hopefully very, very soon :). If you're building from source and would like to test out the current state, the branch is right here.

Also thanks for sharing the solutions other people are using, it's super helpful for development!

[exhaustin]

End-effector tracking improvements has been merged to main and uploaded to anaconda cloud! Closing the issue now, feel free to reopen if you are still experiencing issues with the end-effector accuracy.

[AlexanderKhazatsky]

Could you share the inertial fields that you use for the franka when the robotiq gripper is mounted?

[1heart]

https://github.com/frankaemika/external_gripper_example/blob/master/panda_with_robotiq_gripper_example/config/endeffector-config.json works pretty well

[AlexanderKhazatsky]

Thank you!!