search

justagist / panda_robot

A ROS python interface for using the franka robot (requires franka_ros_interface). Simplified control and management API.
https://justagist.github.io/panda_robot
Apache License 2.0
85 stars 17 forks source link

Issues with ik solver when creating a trajectory with a path of EE poses #8

Closed karthikm-0 closed 3 years ago

karthikm-0 commented 3 years ago

Hi,

I have a strange issue using the ik solver included in this package. I have a path with end-effector poses generated by a PROMP (paper: http://eprints.lincoln.ac.uk/id/eprint/25785/1/5177-probabilistic-movement-primitives.pdf). However, upon attempting to use the solver to convert each pose to joint angles, the solver doesn't always succeed, despite the path of poses being feasible (it is a replay of a trajectory provided as a demonstration with teleoperation). This means I am unable to plan a full trajectory with my poses, but instead have gaps (some poses are skipped as no solution is found). For instance, in a path with 100 poses, at least half of them failed to produce a valid joint configuration, when using seeding for all except the first pose. The situation is the same when using the neutral position as the seed for the first pose. With this approach, the produced trajectory usually does not resemble the demonstration at all, despite the poses of the demonstration and generated aligning perfectly.

However, I have found a workaround for this, which involves taking each individual pose, requesting an ik solution and executing it immediately using the panda_arm interface using exec_position_cmd. When doing this, there are 0 failures (1 at most). Unfortunately, such an approach means the trajectory cannot be planned nor smoothed before execution.

Any insight into why this is occurring would be super helpful. I can also provide the code I'm using when requesting the ik solutions and also provide json demonstration files if they can be of use.

Thanks!

EDIT: I also noticed this when using a DMP so this issue seems to persist regardless of how the EE poses are generated.

justagist commented 3 years ago

Hi, First of all, this package is only providing an interface for using the PyKDL library for kinematics computations and not implementing any of the optimisers or solvers. So I cannot provide any direct support for issues you have when using it. You may find it more helpful to post your questions in their relevant discussion pages.

However, this is my comment: As I tried to explain in https://github.com/justagist/franka_ros_interface/issues/22#issuecomment-783253048 for the issue you raised earlier, IK solvers are not the best option for doing plan execution in the task-space. The solutions may saturate or the optimisers can give no solution if any of its stopping criteria are met (max number of iterations, max time, etc.). This will most likely be the case whichever IK solver you use, although you are welcome to try any solver you want. It may be possible to improve the optimisation and solutions from the solver by changing several hyperparameters for the solver (see documentation of solver package), but this will most likely result in delays.

What you want is to write a higher level task-space controller for your robot that follows goal positions for the end-effector. You can write any type of task-space controller (position, velocity, impedance, etc.) using any of the provided low-level controllers. As a reference, you may want to take a look at the task-space control example in PandaSimulator package, where a simple PD controller for the end-effector is written over the low-level joint-torque controller. This is a very simple example, and you may want to write a more elaborate controller for handling singularities and controlling the null-space in general.

Another important point to note is having a smooth plan is not a guarantee for smooth execution; your controller should take care of these small discontinuities by smoothing it out (for eg. by running at a higher rate than the trajectory generator, or having other smoothing filters).

-- Saif

justagist commented 3 years ago

Also, I just realised you were trying to find IK solutions for a sequence of poses that may be far from the robot's actual current joint positions. This would also result in not finding a good solution in time. The solver needs to know the current mapping between joint positions and end-effector pose to have a decent start with the search in the state-space. A good seed solution in the form of joint positions that are near the goal (for eg. the solution from the IK request for the previous end-effector target) may help, but still does not guarantee a solution.

karthikm-0 commented 3 years ago

Thank you for the detailed comments, @justagist. I am looking into writing a task space controller right now. With regards to your second comment, this was the source of my confusion, but I am sheepish to admit that it was an error on my part. It turns out that when seeding, the default values were set to 0 for all joints for the first pose, but the panda has 2 joints where this is an invalid configuration, which caused the solution to be incorrect from the beginning. This is why when finding the ik solution for a single pose and executing the joint command to move the robot, the issue was not prevalent, but it was when planning the sequence of poses. This has been corrected and is no longer an issue. Thanks again!