Transform3f RevoluteJoint::getLocalTransform(const JointConfig& cfg) const { Quaternion3f quat; quat.fromAxisAngle(axis_, cfg[0]); return Transform3f(transform_to_parent_.getQuatRotation() * quat, transform_to_parent_.getTranslation()); }
Quaternion3f InterpMotion::deltaRotation(FCL_REAL dt) const { Quaternion3f res; res.fromAxisAngle(angular_axis, (FCL_REAL)(dt * angular_vel)); return res; }