Eigen::MatrixXd center_of_mass_
Eigen::MatrixXd relative_position_
double joint_acceleration_
Eigen::MatrixXd joint_axis_
Eigen::MatrixXd position_
Eigen::Matrix4d getTransformationXYZRPY(double position_x, double position_y, double position_z, double roll, double pitch, double yaw)
Eigen::Vector3d getTransitionXYZ(double position_x, double position_y, double position_z)
Eigen::Matrix3d convertRPYToRotation(double roll, double pitch, double yaw)
Eigen::MatrixXd orientation_
Eigen::Matrix3d getInertiaXYZ(double ixx, double ixy, double ixz, double iyy, double iyz, double izz)
Eigen::MatrixXd transformation_