search

Woolfrey / software_robot_library

Custom classes for robot control.
GNU General Public License v3.0
2 stars 1 forks source link

Add calculations for floating base dynamics #89

Closed Woolfrey closed 8 months ago

Woolfrey commented 1 year ago

Need to add calculations to compute the dynamics for floating base systems.

Woolfrey commented 1 year ago

Added these to KinematicTree:

// Compute inertial coupling between the base and links
this->_jointBaseInertiaMatrix.block(0,0,k+1,3) += mass * Jv.transpose();
this->_jointBaseInertiaMatrix.block(0,3,k+1,3) += Jw.transpose()*this->base.inertia()
                                        - mass * (SkewSymmetric<DataType>(currentLink->center_of_mass() - this->base.pose().translation()) * Jv).transpose();

this->_jointBaseCoriolisMatrix.block(0,3,k+1,3) += Jw.transpose()*this->base.inertia_derivative()
                                        - mass * (SkewSymmetric<DataType>(currentLink->twist().head(3))*Jv).transpose();

But still need to check if the calcs are correct. Can probably cross-check the results using the parody_mark1 robot from Clyde Webster (◔ヘ◔)

Woolfrey commented 8 months ago

Closing this as complete, but the numbers still need to be verified.

This is the output when running ./Model/urdf_test ../Misc/parody_mark1.urdf:

image