Implement processor bootstrapping

This bootstrap is required for IMU processor, but some code needs to be inserted in core for it to work.

Please read the bootstrap details on the IMU plugin issue 29: mobile_robotics/wolf_projects/wolf_lib/plugins/imu#29 (closed)

For what concerns the core plugin, here are the necessary changes. They concern ProcessorMotion, Problem, all nodes having state blocks, and StateBlock:

1. ProcessorMotion:
   • Add a member bool ProcessorMotion::bootstrapping to control the following:
     • factors added are declared inactive --> probably in Derived::emplaceFactors()
     • bootstrap() is called
   • Add virtual void ProcessorMotion::bootstrap() method (virtual or pure virtual, TBD)
     • wait for the conditions to apply the bootstrap
     • compute the necessary elements
     • transform all the nodes in Problem with the new bootsrapped solution
     • re-activate all factors that were declared inactive
     • clear the bootstrapping flag
2. Problem:
   • Add void Problem::transformAll(VectorComposite("PO")) to transform all wolf tree after bootstrapping
3. Nodes (maybe in HasStateBlocks):
   • Add void AllNodes::transform(VectorComposite("PO")) to transform all node's state blocks
4. StateBlock:
   • Add void StateBlock::transform(VectorComposite("PO")) = 0 to transform the state block
   • Add bool StateBlock::transformable to control if transformation should be applied
   • Derive all state blocks to specific types.
   • Implement transform in all derived state blocks

added Agreement required Code organization Doing External API update enhancement labels

assigned to @jsola

changed the description

mentioned in issue mobile_robotics/wolf_projects/wolf_lib/plugins/imu#29 (closed)

changed the description

changed the description

marked the checklist item Add a member bool ProcessorMotion::bootstrapping to control the following: as completed

marked the checklist item bootstrap() is called as completed

marked the checklist item Add virtual void ProcessorMotion::bootstrap() method (virtual or pure virtual, TBD) as completed

changed the description

marked the checklist item factors added are declared inactive --> probably in Derived::emplaceFactors() as completed

marked the checklist item Add void Problem::transformAll(VectorComposite("PO")) to transform all wolf tree after bootstrapping as completed

marked the checklist item Add void AllNodes::transform(VectorComposite("PO")) to transform all node's state blocks as completed

marked the checklist item Add bool StateBlock::transformable to control if transformation should be applied as completed

@joanvallve regarding the point: "Derive all state blocks to specific types", we can condensate these "specific types" to some family, e.g.

1. StateNonTransformable: states that are not transformed, no matter what
2. StateVector: vectors that transform with a pure rotation
3. StatePoint: vectors that transform with rotation and translation
4. StateQuaternion, Angle, Homogeneous: specific types, they exist already
5. StateBlaBla: any new state block that may appear in the future

Do you see any inconvenient in doing this?

Note: in any case, the property StateBlock::transformable and method StateBlock::isTransformable() exist to block the capacity of transforming the object. So types 1,2,3,4 can be declared non-transformable too.

Possibly, after defining the types above, we can typedef them to more specific types:

typedef StateVector StateVelocity;
typedef StateVector StateAcceleration;
typedef StateVector StateAngularVelocity;
typedef StateVector StateAngularMomentum;
typedef StatePoint StatePosition;
typedef StatePoint StateCoM;                       // this one is really stupid, but just to illustrate
typedef StateNonTransformable StateIntrinsic;
typedef StateNonTransformable StateBias;
typedef StateNonTransformable StateGains;
typedef StateNonTransformable StateDiffDriveModel; // this one is really stupid, but just to illustrate

Not sure if typedef is enough, I think we need to register them to the factory.

Also, I would vote for specifying StatePoint2d and StatePoint3d (and the same for the rest).