getting started?

[ahundt]

Is there any documentation or a good way to get started with this library?

[jorisv]

Hello,

Sorry but right now PG is completely undocumented.
If you think this library might be useful for you, I can try to add some build instructions/doxygen/tutorials. But first, let's see if this library is what you want.

PG doesn't:

• Solve a nonlinear problem. Instead, this library creates the matrices (functions and Jacobian) that describe the IK problem and uses roboptim as a bridge with nonlinear solvers (I was using IPOPT as a solver with roboptim-core-plugin-ipopt).

PG supports:

• Revolute/Prismatic/Planar/Cylindrical/Fixed joints. Free and Spherical joints are not supported (because of the nonlinear solver). Instead, we transform one contact of the robot as a Planar/Cylindrical/Fixed joint to simulate the Free joint.
• Static equilibrium.
• Contact forces in a friction cone.
• Articular limits.
• Collision avoidance.
• Multi-contacts (planar, cylindrical, fixed).
• Robot chaining: you can solve the IK for N robots in the same nonlinear problem and connect contacts of adjacent robots. This allows to find optimal contact placement while moving in an environment.
• Minimization of: contact force of some contacts, distance to a targeted posture, position/orientation error for some robot link, tangential force of some contacts (increase stability), normal force error of some contacts.

This library has been used successfully for posture/contact configuration planning of humanoid robots.

[ahundt]

Wow that sounds fantastic! I'm using a 7dof arm with all revolute joints, the kuka lbr iiwa. Right now I have existing code that follows a path in cartesian space, but it uses a simple pseudoinverse to solve the inverse kinematics, and thus has the potential to get caught.

What I'm looking to do is to improve the planning with the extra degree of freedom to avoid joint limits and singularities better, plus ideally optimize the end effector to move with a constant contact force. (cutting)

Also your description sounds like a great addition to the readme.md. Thanks!

[jorisv]

Do you want to do control or planning?

PG is designed to compute static robot postures but is not designed to generate robot motions.
I use it to plan (off-line) postures and contact positions for humanoid robots evolving in some complex environments.
Then I use the Tasks library to generate real-time motions with static postures and contacts previously computed as targets.

The Tasks library is designed to make real-time control and have the following features:

• Support Kinematics Tree with Revolute/Prismatic/Spherical/Free/Planar/Cylindrical joints
• Dynamic motion (motion must fulfill the equation of motion)
• Contacts forces in friction cone
• Static contacts
• Articular position, speed and torque limits
• Collision avoidance
• Multi-robot (can solve a problem involving multiple robots in contact)
• Tasks:
  • Posture target (articular position target, mandatory if you want avoid singularity issues)
  • Link Position/Orientation target
  • Link Velocity target
  • CoM target
  • Momentum target
  • Contact force target (only useful if you have a torque controlled robot)

Also this library is far more stable than PG because it uses a nice and stable QP to solve the problem instead of a "nonlinear solver of doom".

Unfortunately, like PG, this library is undocumented. Also, another issue is that you cannot compile it without the LSSOL library that is proprietary. Building without it should be possible, but it will necessitate some work in the build chain.
I have plans to write documentation/tutorials for it in the future because some of my colleagues need it.

[ahundt]

Unfortunately my task doesn't fit 100% strictly into either category. The vast majority of the motion I need to follow is a pre-planned path, only the initial approach and final departure steps may involve planning, though even that planning can be ignored for my first version.

To keep the explanation simple, I included an image below. The robot arm has a cutter, it approaches a cut path which is the very light blue spline inside the bone image in the top right. The cutter follows that path, clearing out the interior of the bone. When the motion is complete it is removed.

I need to approach the path and follow it with the following "constraints":

• 7R redundant arm, want to try different options for placing the remaining degree of freedom
• The bone may move which I have to account for in real time (though this will be uncommon in real situations)
• Maintain constant cutting pressure because bone material density varies dramatically and moving quickly in low density areas is key.
  • The arm has torque sensors in each joint to measure the externally applied torque in real time
• Avoid joint limits of the arm
• Avoid singularities
• Optionally avoid collisions (this is low priority and can be ignored)

So the most critical part is that I am following a path with a fully specified 6dof end effector position and orientation, and I need to optimize the motion and the force applied. It probably makes the most sense to continuously adjust rather than plan over the whole path.

A bonus would be if I could verify that the entire cut path is reachable before I start.

Do you think either library would be suitable for that task?

[jorisv]

I think Tasks is a better choice for your problem.

• 7R redundant arm, want to try different options for placing the remaining degree of freedom
• Avoid singularities

I used to manage both points by using a Posture Task (targeting a non singular articular position) with low priority. This allow to always have all degree of freedom used and avoid singularities issues.

• The bone may move which I have to account for in real time (though this will be uncommon in real situations)

Here there is a lot of way to manage it. You can update the position target of your cutter. Another way is to model the bone as a unactuated robot, then you can specify the cutter position target relative to the bone (more complex solution).

• Maintain constant cutting pressure because bone material density varies dramatically and moving quickly in low density areas is key.

During motion you can target a reference position/velocity/acceleration so you can manage the velocity of your cutter.
To manage pressure I would use impedance control. This way of control will give a targeted velocity based on the error between a desired contact force and a force sensor output.

• Avoid joint limits of the arm

Managed

• Optionally avoid collisions (this is low priority and can be ignored)

Managed against convex hull

So the most critical part is that I am following a path with a fully specified 6dof end effector position and orientation, and I need to optimize the motion and the force applied. It probably makes the most sense to continuously adjust rather than plan over the whole path.

I already used Tasks to do 6dof path tracking (using a B-spline has path) and to make impedance control (apply a constant force on a target). This library will give you low level functions to solve those issues. There is not a class that will allow you to make out of the box path tracking but instead some tasks (like the end effector 6 dof task) with position/velocity/acceleration target.

A bonus would be if I could verify that the entire cut path is reachable before I start.

That's a difficult issue. I would run the controller off line and compute the tracking error to decide if the path is reachable or not.

Now the big issue is more on the time you have to do that. The Tasks library can't be compiled right now because of a missing dependency. Also she's not documented. Solving both issue can take me 2 weeks.

[ahundt]

Ok, well I'm trying out a few possible alternatives and planning to make v-rep plugins for them in my own library grl, so that will all take time and thus I can wait a bit.

In case you are both interested and aren't yet aware of them the one from others in my lab is cisst-saw, particularly the sawConstraintController component. Another is versor which implements the fabrik ik algorithm in its vsr_chain header and I have a similar conversation going on there.

The approaches can differ quite a bit so I'm trying to learn about a broad range.

[ahundt]

I wanted to follow up, is there any chance of solving the dependency issue and adding a bit of starter documentation so I could try the library out?

[jorisv]

Sorry I was focused on something else.
I will try to solve the dependency issue around Wednesday and also add some doxygen documentation.

[ahundt]

Thanks!

[jorisv]

All Tasks dependencies are now available.
I will add some more doc until the end of the month.

[ahundt]

Great, thanks! I'm traveling at the moment but I'll start giving it a try after I'm back.

[ahundt]

Tasks looks like the way to go, I created jrl-umi3218/Tasks#10 to continue relevant discussion.