Enhancements cob_twist_controller

[fmessmer]

ToDo:

• investigate reset of PositionInterface (there are large jerks after no twist has been received for a while, i.e. vel vectors for simpson need to be cleared and then filled afresh!)
• add/test/optimize TrajectoryInterface (see fmessmer@a740835); reset needs to be considered here, too! 👉 related to [cob_twist_controller] verify integration behavior and moving average (for position controller interfaces) #66
• add kinematic extension for Torso 👉 needs more testing
• add kinematic extension for Lookat 👉 done in TwistController Updates #63
• investigate synergies between cob_obstacle_distance and added cob_collision_monitor package ipa320/cob_manipulation#54 👉 moved to new separate issue [cob_twist_controller] CollisionAvoidance constraint does not work with KinematicExtensions #64

[fmessmer]

Other ToDos:

• https://github.com/ipa320/cob_control/pull/51#discussion_r37741429
• Parameter config updates cob4 2 ipa320/cob_robots#322 (comment) 👉 DONE in fmessmer@25eb206

[fmessmer]

Investigate whether GPM movements are actually self-motions, i.e. does the TCP actually not move in case main task commands zero-twists?
If GPM movements are no actual self-motions, i.e. the result in the TCP moving in Cartesian space, we need to investigate whether we use the correct Jacobian in the projection (I - J_inv*J)? Should the J there be the pure kinematic Jacobian or the damped/modified Jacobian?

[fmessmer]

Did some more tests on GPM and self-motion:

In fmessmer@825504f I added code for calculating FK_Vel (i.e. CartVel = J*q_dots_out) in order to see whether twist resulting from the commanded velocities would be (close to ) Zero Twist.

I take this as a proof for that the homogeneous part actually results in a self-motion.
The particular_solution is all 0.0 as I am commanding a ZeroTwist. The homogeneous_solution is non-zero as I was moving an interactive obstacle towards the arm with GPM enabled. One examplary iteration gives the following values. Also, when moving slowly and giving the arm the chance to "escape", the tip does actually not move on the real hardware.
The following output is computed using: Eigen::MatrixXd projector = ident - jpi * this->jacobian_data_;, i.e. the inverse of the undamped Jacobian

[ INFO] [1443091881.204137450]: particular_solution: 0 , 0 , 0 , 0 , 0 , 0 , 0 , 
[ INFO] [1443091881.204247496]: homogeneous_solution: 0.00918778 , -0.00108638 , 0.0016462 , -1.72113e-17 , -0.0231118 , -0.00593743 , 0.0161203 , 
[ INFO] [1443091881.204315259]: resultingCartVelocities: -1.69229e-18 , 3.15874e-19 , -4.03527e-18 , -2.51535e-17 , 2.42861e-17 , 4.16334e-17 , 

The following output is computed using Eigen::MatrixXd projector = ident - damped_jpi * this->jacobian_data_;, i.e. the inverse of the damped Jacobian

[ INFO] [1443092841.083227550]: particular_solution: 0 , 0 , 0 , 0 , 0 , 0 , 0 , 
[ INFO] [1443092841.083320793]: homogeneous_solution: 0.00893877 , -0.00126842 , 0.00129907 , -4.53435e-17 , -0.0502214 , 2.63773e-06 , 0.043587 , 
[ INFO] [1443092841.083395728]: resultingCartVelocities: 4.59222e-18 , 2.85462e-17 , -9.06262e-18 , -2.77556e-17 , -8.67362e-17 , -3.53884e-16 ,

Both tests have been performed with JLA_OFF!
Thus, I'd say both variants (i.e. using damped_jpi in the projector as well as jpi lead to self-motions).
An issue regarding JLA can be found in #56

[fmessmer]

Just found that the velocityies q_dots_out (which actually are just the homogeneous_solution from the GPM) are not executed exactly by the hardware (dashed is the command, solid is the actual joint_velocity):

commands-only:

joint_velocities only:

This results in a non-zero Twist

For Schunk LWA4d this might be due to very slow acceleration limits configured in the firmware!
However, this fact shows that, although velocity limits are guaranteed (see plateaus in the dashed commands), it is not guaranteed that these velocities can actually be executed due to too high accelerations!

Thus, we need acceleration scaling (i.e. acceleration limiters) as well!!!

[fmessmer]

Summary of above investigations:

• self-motions are self-motions
• execution has been faulty due to faulty limit interfaces (see https://github.com/ipa320/cob_control/issues/57 for discussion)
• we can live with the current implementation/solution...

EDIT:

• re-verify that self-motions are self-motions in case damping is significant - use constant damping (See discussion with @ipa-fxm-mb and respective octave scripts
• apply fixes to all solvers (i.e. GPM, StackOfTasks, TaskPriority)

👉 new issue to track self-motion issue #65

[fmessmer]

Enhancement regarding 'kinematic_extensions':

• try to use common update_rate throughout all framework (i.e. cob_cartesian_controller, cob_frame_tracker, cob_twist_controller) 👉 moved to [cob_twist_controller][cob_frame_tracker][cob_cartesian_controller] Verify/Consistency for TimeStamps, Timing and TF-Listening #76
• figure out how to properly use base_ratio (rename to extension_ratio)
• extensions should "pop" their "share" of "q_dot_out" so that the main chain only receives its own q_dot_out" (i.e. size equals chain_dof) 👉 done here
• extensions should be able to "modify" limiters in order to consider limits of extensions properly, without hardcoded limits as e.g. for base here 👉 started/unstable here 👉 done in TwistController Updates #63
• extensions should be considered within constraints 👉 started/unstable here done in TwistController Updates #63

[fmessmer]

More thoughts on:
How to deal with the fact that chain extends from the main chain tip onwards rather than before the main chain base?

• Additional FrameTrackerService StartLookat which transforms between virtual lookat_tip and target rather than chain_tip and target
• Additional Menu-Entry in InteractiveMarker for StartLookat
• Lookat-Extension would need to deal with "virtual" JointStates handling (i.e. vel integration and member variable) as well as tf_frame broadcasting (for cob_frame_tracker to be able to get according transform)
• Lookat-Extension needs to be configurable wrt. which axis of the chain_tip coordinate system to use as lookat_lin_joint

👉 lookat has been implemented like that in #63

[fmessmer]

constraints need to be implemented in a way that they can be used with extensions!
⚠️ proper dimension handling (KDL::solver initialized with primary chain vs. JointStates of extended dim)

Still not solved!
👉 new issue for tackling this issue separately #64

[fmessmer]

Think about timing....
During a solveTwist() cycle, several times (i.e. in constraints, transforms,...) a ros::Time::now() is used...
Is this correct? Should we rather provide a ros::Time variable within the DataStructure that is set to now() on arrival of the twist and then that variable is used instead of again calling now?

[fmessmer]

For now, most issues and enhancements have been resolved or are now tracked in separate issues #64 #65