Cartesian Pose Control as directly supported by Panda

[jk-ethz]

Hi,

I know this is related to #17 and task-space cartesian pose control can be achieved via torque control and a PD controller.

However, the Panda HW interface also supports direct cartesian pose control.

This is implementable in a pretty straightforward manner in franka_ros_interface by subscribing for example to a TF topic like /franka_ros_interface/motion_controller/arm/cartesian_pose_commands.

However, it would also be interesting to have this functionality in the simulator, for example in a file here. Given that I don't have a Panda at the moment to find out how the real one behaves with the cartesian interface, I can not implement this simulation functionality right now.

Is it planned to also support this in simulator or would you always use the route of torque control (supported in simulation already) + PD controller. If so, this is the only example I found so far, is there also a C++ version already existing for it?

[justagist]

Hi @jk-ethz, thanks for your interest! I've moved this post from "issues" to "discussion" as it seems more appropriate here.

Now addressing your queries.

Is it planned to also support this in simulator or would you always use the route of torque control (supported in simulation already) + PD controller.

Short answer: This is not in my plans at the moment.

This is implementable in a pretty straightforward manner in franka_ros_interface by subscribing for example to a TF topic like /franka_ros_interface/motion_controller/arm/cartesian_pose_commands.

Like you said, it is possible to add another controller code in franka_ros_interface and publish commands via ROS topic to control the cartesian pose of the robot directly because the robot supports this natively. This will work fine if you want to directly control the robot through ROS topics. Unfortunately, it is not quite straightforward to implement this without breaking other parts of the franka_ros_interface API. The main intention of this package is to provide a ROS/Python API for controlling the robot, and implementing direct support for cartesian control will require refactoring and restructuring several parts of the library including the controller manager interface. This might take more time than I can currently afford to spend. This is definitely on the list of things that I want to implement at some point, however. Any help on this front is most welcome!

However, it would also be interesting to have this functionality in the simulator, for example in a file here. Given that I don't have a Panda at the moment to find out how the real one behaves with the cartesian interface, I can not implement this simulation functionality right now.

This is a more interesting problem. These are some of the issues that are stopping me from implementing a cartesian controller for the simulator:

1. I am not sure how the cartesian controller is implemented on the physical Panda robot. So I am not sure we can guarantee the same controller behaviour as the physical robot in simulation. This would mainly depend on the kind of IK solver (or fast dynamics solver) they use for the computation and how they are handling the null-space of the robot. So unless I know exactly how their controller was implemented, I will end up writing a controller for the simulator which might be very different from the implementation on the real system; I might get end-effector tracking to match, but the joint configurations may not. So you might as well ignore how the real robot behaves and try implementing the simulation controller yourself if you want.
2. As far as I know, ROS doesn't directly provide cartesian controllers in the ros_control architecture, which means I will have to either use some external library that has such controllers for ROS (if there is one; I haven't looked for any yet, so I don't know much about this option) or write my own controller on top of one of ros_control's low-level joint controller types, which will then, in the end, be similar to (not exactly the same as) the PD control example you linked to.
3. I believe it is easier to match the behaviour of a cartesian controller on real and simulated when using a low-level internal torque controller (if you use the same control law and null-space control; the gains might have to be changed, of course).

(I would look into implementing a cartesian controller for direct sim control only after I have incorporated cartesian control support in the franka_ros_interface API anyway.)

If so, this is the only example I found so far, is there also a C++ version already existing for it?

I don't have a C++ version, sorry. But it should be straightforward to write a similar example in C++. Another version of the same example that you linked is this. This is also a python example, but it uses ROS topics to control the robot instead of the API. You should be able to control the robot in the same manner by publishing and subscribing to the same topics as in this example. Hope it helps.

[jk-ethz]

I agree that the cleanest would be to have it compiled directly in Unreal, but the specifics and incompatibilities with conventions of the Unreal compiler make this just a nightmare, so an external wrapper might be the way to go.

[justagist]

Is it suitable for executing large movements and also for many small movements with the target marker without causing jumpy behavior?

Moveit has planners and controllers that executes a motion if you provide a target (or a sequence of targets); the target can be in the joint-space or task-space. These will be smooth motion because they interpolate the trajectory to generate jerk-free motion (but there may be other situations where MoveIt might fail, such as if it is unable to find a feasible plan; although such scenarios come up mainly when the robot is in a cluttered environment, or when the target sequence provided are too complex; there are very unlikely in simple situations).

I agree that the cleanest would be to have it compiled directly in Unreal, but the specifics and incompatibilities with conventions of the Unreal compiler make this just a nightmare, so an external wrapper might be the way to go.

Moveit provides an interface in Python and C++. I don't have much experience with Unreal but I think you may be able to directly use the C++ interface in the code. This link might be a good starting point to get yourself familiar with a working example in C++. You can also find the same example in using python interface in the next section of the tutorial. (You may want to look at the previous pages of the tutorial for instructions on set up and to get more familiar with the concepts.)

[jk-ethz]

Moveit has planners and controllers that executes a motion if you provide a target (or a sequence of targets); the target can be in the joint-space or task-space. These will be smooth motion because they interpolate the trajectory to generate jerk-free motion (but there may be other situations where MoveIt might fail, such as if it is unable to find a feasible plan; although such scenarios come up mainly when the robot is in a cluttered environment, or when the target sequence provided are too complex; there are very unlikely in simple situations).

That sounds good. I'll go try that.

Moveit provides an interface in Python and C++. I don't have much experience with Unreal but I think you may be able to directly use the C++ interface in the code. This link might be a good starting point to get yourself familiar with a working example in C++. You can also find the same example in using python interface in the next section of the tutorial. (You may want to look at the previous pages of the tutorial for instructions on set up and to get more familiar with the concepts.)

Very nice, thanks for your support and helping me in my decision. I am now convinced it's the most future proof way to go with Moveit, as there may also be the need for collision avoidance in the future and Moveit has that right out of the box.

[siweiyong]

@justagist Hi, thank you very much for providing the panda interface.

I found a weird problem when I use provided demo "task_space_contrl_with_fri"(https://github.com/justagist/panda_simulator/blob/melodic-devel/panda_simulator_examples/scripts/task_space_control_with_fri.py). As you provided the controller, based on the position error in cartesian space, using a PD controller to calculate the desired force in cartesian space. Then use the torque controller in the joint space. I found it can not accurately control the cartesian pose by this controller. For example, when I only track motion （constant velocity）in the X direction (I set the delta_pos[0] =0.01 delta_pos[1] =0.0 delta_pos[2] =0.0 in the control_thread(). ).I expected the robot to only move along the X direction, but the robot also moves in the Z direction (the position in Z decreases).
When I test on other directions, it has the same issue. Both in the simulator and the real robot, the controller has the same problems. Thank you very much!

[justagist]

@justagist Hi, thank you very much for providing the panda interface.

I found a weird problem when I use provided demo "task_space_contrl_with_fri"(https://github.com/justagist/panda_simulator/blob/melodic-devel/panda_simulator_examples/scripts/task_space_control_with_fri.py). As you provided the controller, based on the position error in cartesian space, using a PD controller to calculate the desired force in cartesian space. Then use the torque controller in the joint space. I found it can not accurately control the cartesian pose by this controller. For example, when I only track motion （constant velocity）in the X direction (I set the delta_pos[0] =0.01 delta_pos[1] =0.0 delta_pos[2] =0.0 in the control_thread(). ).I expected the robot to only move along the X direction, but the robot also moves in the Z direction (the position in Z decreases).
When I test on other directions, it has the same issue. Both in the simulator and the real robot, the controller has the same problems. Thank you very much!

Moved this comment to issues (#49)

[mitsui29]

@jk-ethz hi, i just have a couple of questions about this issue. i wanna try my own impedance controller in c++, first on the gazebo simulation and then on real panda. Maybe just under the path: panda_simulator_examples/src. It should similar to the file task_space_control_with_fri.py. The simple way is using topics and service to control the robot. But there are so many barriers from arm.py and gripper.py. I mean the interfaces something like that. The another is using plugin(see the issue #25). Did u try your controller ? Thanks a lot!

[jk-ethz]

I ended up using MoveIt for the planner and didn't make use of the cartesian pose controller at all. This worked quite nicely and I didn't have to dig into the details of the impedance controllers. So I don't have experience with the impedance control implementation.