Towards a type that is analogous to base::samples::Joints

[shivesh1210]

Ideal features in the rigid body SE(3) type:

0. Body Frame Name
1. Pose, SE(3) - R^3 + SO(3)
2. Spatial Velocity/Twist, se(3) - R^6
3. Spatial Acceleration - R^6
4. Wrench se*(3) or R^6
5. optional raw field can be kept R^6

The above convention assumes that the information is stored either in body frame or inertial frame of reference. To be more general, one could add an additional field: source frame but then we are not completely analogous to base::samples::Joints.

We should be able to create a named vector of this type so that we don't have multiple input ports carrying information about left limb, right limb, left arm, right arm etc. In this regard, we face a design choice:

1. all the elements of this vector share the same source frame
2. each element in the vector is allowed to have its own source and target frame name definition

What do you think is a better choice?

Regarding the name, I would propose RigidBodyStatesSE3.

[dmronga]

I agree in principle, comments & questions inline

Ideal features in the rigid body SE(3) type:

1. Body Frame Name

2. Pose, SE(3) - R^3 + SO(3)

I would keep here the base/Pose type!?

3. Spatial Velocity/Twist, se(3) - R^6

I would keep here the Twist type from control-ctrl_types!?

4. Spatial Acceleration - R^6

Same as above

5. Wrench se*(3) or R^6

Use base/Wrench type here!?

6. optional raw field can be kept R^6

I don't know about this. This opens the door to a lot of messing around.
Pro: Analogy to base/samples/Joints
Con: Blows up the data types unnecessarily. And I don't see a use case. In base/samples/Joints the raw field can at least be used for PWM.

How about a timestamp?

The above convention assumes that the information is stored either in body frame or inertial frame of reference. To be more general, one could add an additional field: source frame but then we are not completely analogous to base::samples::Joints.
Yes. And the target_frame -> source_frame convention we use in rock is a constant source of misinterpretation. However in many applications in WBC I need both, the source frame and the target frame. I will check if I can get rid of it somehow.

We should be able to create a named vector of this type so that we don't have multiple input ports carrying information about left limb, right limb, left arm, right arm etc. In this regard, we face a design choice:

1. all the elements of this vector share the same source frame

2. each element in the vector is allowed to have its own source and target frame name definition

What do you think is a better choice?
If we use base::NamedVector as a parent class, we have already one name per element. And that would be in addition to the source frame (and eventually target frame) in the base type itself. So the only way to share a common source frame would be to create a new type that contains a string with the common frame name and the vector of poses. A lot of doubling here... So I would opt for 2.

Regarding the name, I would propose RigidBodyStatesSE3.
Personally I know only approximately what SE3 is. At least I dont understand exactly the difference to the Euclidean space. Other people will not know either. Any other suggestions?

[shivesh1210]

I agree in principle, comments & questions inline

Ideal features in the rigid body SE(3) type:

1. Body Frame Name

2. Pose, SE(3) - R^3 + SO(3)

I would keep here the base/Pose type!?

Yes, its fine.

3. Spatial Velocity/Twist, se(3) - R^6

I would keep here the Twist type from control-ctrl_types!?

Yes, its fine.

4. Spatial Acceleration - R^6

Same as above

Yes, its fine.

5. Wrench se*(3) or R^6

Use base/Wrench type here!?

Yes, its fine.

6. optional raw field can be kept R^6

I don't know about this. This opens the door to a lot of messing around.
Pro: Analogy to base/samples/Joints
Con: Blows up the data types unnecessarily. And I don't see a use case. In base/samples/Joints the raw field can at least be used for PWM.

Okay, we can skip it. I also can not think of a useful application of the raw field.

How about a timestamp?

This would be nice to have. I guess RigidBodyState also has it, right?

The above convention assumes that the information is stored either in body frame or inertial frame of reference. To be more general, one could add an additional field: source frame but then we are not completely analogous to base::samples::Joints.
Yes. And the target_frame -> source_frame convention we use in rock is a constant source of misinterpretation. However in many applications in WBC I need both, the source frame and the target frame. I will check if I can get rid of it somehow.
We should be able to create a named vector of this type so that we don't have multiple input ports carrying information about left limb, right limb, left arm, right arm etc. In this regard, we face a design choice:

1. all the elements of this vector share the same source frame

2. each element in the vector is allowed to have its own source and target frame name definition

What do you think is a better choice?
If we use base::NamedVector as a parent class, we have already one name per element. And that would be in addition to the source frame (and eventually target frame) in the base type itself. So the only way to share a common source frame would be to create a new type that contains a string with the common frame name and the vector of poses. A lot of doubling here... So I would opt for 2.
I would say that the name per element could be the target frame. We can provide an optional source frame variable for every element which when not set means the element is carrying a transformation in the inertial or world frame of reference. When set explicitly carries the information about the source frame.
Regarding the name, I would propose RigidBodyStatesSE3.
Personally I know only approximately what SE3 is. At least I dont understand exactly the difference to the Euclidean space. Other people will not know either. Any other suggestions?

SE(3) is the special Euclidean group with dimension 3. This group has 6 degrees of freedom: 3 rotational and 3 transnational. It could be seen as a semi-direct product of special orthogonal group SO(3) and R^3. It is called special because we choose det(R) = +1 reflecting our choice of right hand coordinate system. You can read appendix A.3 of my thesis for more information: https://elib.suub.uni-bremen.de/edocs/00107793-1.pdf
I would really push for group theoretic data types so that the operations on these types are clearly defined (for e.g. only multiplication between two SE3 elements makes sense). Most of the modern robotics is built on the mathematical foundations of group theory and differential geometry and we should stop using ad-hoc descriptions.

[dmronga]

OK, in summary I would suggest the following new types

namespace base{

class Twist{
public:
base::Vector3d linear; /** Linear 3D vel (m/s)/
base::Vector3d angular; /** Angular 3D vel (rad/s)/
};

class Acceleration{
public:
base::Vector3d linear; /** Linear 3D acc (m/ss)/
base::Vector3d angular; /** Angular 3D acc (rad/ss)/
};

class RigidBodyStateSE3{
public:
Pose pose; /** 3D position / Quaternion orientation (from base/types) */
Twist twist;
Acceleration acceleration;
Wrench wrench; /**3D force / 3D torque (from base/types) */
};

///////////////////////////////////////////////

namespace samples{

class RigidBodyStateSE3 : public base::RigidBodyStateSE3{
public:
base::Time time;
std::string frame_id;
};
// ... analogue for the other types

class RigidBodyStatesSE3 : public base::NamedVectorbase::RigidBodyStateSE3{
public:
base::Time time;
std::string frame_id;
};

} // namespace samples

} // namespace base

Regarding the vector of RIgidBodyStateSE3 we would have a common frame_id for all elements. From base::NamedVector we additionally have the "name" variable, which could be used as source_frame. For example frame_id = robot_base, name = right_hand/left_hand/right_arm ... The only drawback is that you cannot have elements with different reference frames in a vector.

Still I think this is the cleanest solution and we are analogue to base/samples/Joints and also to the ROS conventions e.g. geometry_msgs/PoseStamped or geometry_msgs/PoseArray.

We are not completely analogue to RigidBodyState anymore, but for WBC it should be ok. I don't know about other software like the transformer, but I guess we will never rewrite the transformer using this data type anyways. And we are rid of the confusing source_frame / target_frame convention.

In addition we should provide operators like addition, subtraction, multiplication, dot- / scalar-product etc. within the types itself, to make sure they are used in a mathematically correct way. I already started this is the ctrl-types repo, but there I created the operators "as needed" without claiming mathematical correctness.

What do you think? We will still have to convince the base/types maintainers.

[shivesh1210]

I agree with your proposition. Actually I like this idea that every element in the vector would share a common frame. This actually allow us to define some group operations nicely for e.g. one could query relative transforms between two frames, call for inverse etc. Remember that one can define:

• multiplication and inverse operations for SE3 elements i.e. pose only
• addition is defined for se3 elements i.e. twists and spatial accelerations and se*3 elements i.e. wrenches
• a scalar product is defined for se3 (twist) and se*3 (wrench) elements - expression for work (note: you can not take a scalar product between two twists or two wrenches)
• there are coordinate transforms (plucker transforms) defined for se3 and se*3 elements
• spatial cross product is defined for two elements of se3 and results in acceleration term
  Here is a summary of operations based on Featherstone notation: https://homes.cs.washington.edu/~todorov/courses/amath533/FeatherstoneSlides.pdf
  I don't care which notation we use as long as it is consistent and well documented.

[shivesh1210]

@dmronga I am not sure what you mean by * operator for two twists. Is it the dot product between two twists? If yes, then it is not really defined. See my above comment (3). You can do inner product between twist and wrench which would give you the expression for power but we can not do it for a pair of two twists or two wrenches.

[dmronga]

Are you referring to this line? If yes, this is not supposed to be a dot product, but a component-wise multiplication with a 6D vector. Output is again a Twist

[shivesh1210]

Yes. Why would we need something like this?

[dmronga]

I had it as a helper function in the controllers for conveniently multiplying with the 6x1 control gain vector. However, I can also put it into the control library itself. I'll remove it from base-types

[shivesh1210]

Yes, its better to remove such helper functions from here. Since twists live in a vector space (R^6), we can reserve * operator for scalar-vector multiplication such that (a*t belongs to R^6 where a is a scalar and t is the twist).

[dmronga]

Did you also double check the coordinate transformations? I mean posetwist, poseacc, pose*wrench. Are they correctly implemented?

[shivesh1210]

I can not find them. Can you please point me to the line numbers?

[dmronga]

https://github.com/dmronga/base-types/blob/983f5d789b902d178b7afd67b3bdb476b696b8d3/src/RigidBodyStateSE3.cpp#L36

[shivesh1210]

To me they do not look correct. Can you point me where did you get these formulae from? Please check against the modern robotics textbook or Featherstone.

[dmronga]

I use the implementation from KDL, e.g. https://github.com/orocos/orocos_kinematics_dynamics/blob/5145b19a7ed52cec59af9d6e626cb323f22b1b85/orocos_kdl/src/frames.inl#L274

[dmronga]

I think the formulas are the same as here (slide 15). I will double check that.