31 lambda_q.push_back(0);
32 mu.push_back(std::vector<unsigned int>());
36 previously_added_body_id = 0;
41 v.push_back(zero_spatial);
42 a.push_back(zero_spatial);
45 mJoints.push_back(root_joint);
46 S.push_back(zero_spatial);
49 v_J.push_back(zero_spatial);
50 c_J.push_back(zero_spatial);
53 multdof3_S.push_back(Matrix63::Zero());
54 multdof3_U.push_back(Matrix63::Zero());
55 multdof3_Dinv.push_back(Matrix3d::Zero());
56 multdof3_u.push_back(Vector3d::Zero());
57 multdof3_w_index.push_back(0);
60 c.push_back(zero_spatial);
61 IA.push_back(SpatialMatrix::Identity());
62 pA.push_back(zero_spatial);
63 U.push_back(zero_spatial);
65 u = VectorNd::Zero(1);
66 d = VectorNd::Zero(1);
68 f.push_back(zero_spatial);
72 hc.push_back(zero_spatial);
73 hdotc.push_back(zero_spatial);
79 mBodies.push_back(root_body);
80 mBodyNameMap[
"ROOT"] = 0;
82 fixed_body_discriminator = std::numeric_limits<unsigned int>::max() / 2;
87 const unsigned int parent_id,
89 const Joint &UNUSED(joint),
91 std::string body_name) {
116 if (model.
mFixedBodies.size() > std::numeric_limits<unsigned int>::max()
118 std::cerr <<
"Error: cannot add more than " 119 << std::numeric_limits<unsigned int>::max()
121 <<
" fixed bodies. You need to modify " 122 <<
"Model::fixed_body_discriminator for this." << std::endl;
127 if (body_name.size() != 0) {
129 std::cerr <<
"Error: Body with name '" << body_name <<
"' already exists!" 143 const unsigned int parent_id,
147 std::string body_name) {
151 unsigned int joint_count = 0;
168 std::cerr <<
"Error: Invalid joint type: " << joint.
mJointType << std::endl;
170 assert(0 && !
"Invalid joint type!");
176 unsigned int null_parent = parent_id;
194 Joint single_dof_joint;
200 for (j = 0; j < joint_count; j++) {
213 if (rotation ==
Vector3d(0., 0., 0.)) {
215 }
else if (translation ==
Vector3d(0., 0., 0.)) {
223 joint_frame_transform = joint_frame;
227 if (j == joint_count - 1)
234 joint_frame_transform,
242 joint_frame_transform,
249 const unsigned int parent_id,
253 std::string body_name) {
254 assert(lambda.size() > 0);
266 return previously_added_body_id;
290 return previously_added_body_id;
295 unsigned int movable_parent_id = parent_id;
298 if (IsFixedBodyId(parent_id)) {
299 unsigned int fbody_id = parent_id - fixed_body_discriminator;
300 movable_parent_id = mFixedBodies[fbody_id].mMovableParent;
301 movable_parent_transform = mFixedBodies[fbody_id].mParentTransform;
305 lambda.push_back(movable_parent_id);
306 unsigned int lambda_q_last = mJoints[mJoints.size() - 1].q_index;
308 if (mJoints[mJoints.size() - 1].mDoFCount > 0
310 lambda_q_last = lambda_q_last + mJoints[mJoints.size() - 1].mDoFCount;
311 }
else if (mJoints[mJoints.size() - 1].mJointType ==
JointTypeCustom) {
315 lambda_q_last + mCustomJoints[mCustomJoints.size() - 1]->mDoFCount;
318 for (
unsigned int i = 0; i < joint.
mDoFCount; i++) {
319 lambda_q.push_back(lambda_q_last + i);
321 mu.push_back(std::vector<unsigned int>());
322 mu.at(movable_parent_id).push_back(mBodies.size());
327 mBodies.push_back(body);
329 if (body_name.size() != 0) {
330 if (mBodyNameMap.find(body_name) != mBodyNameMap.end()) {
331 std::cerr <<
"Error: Body with name '" << body_name <<
"' already exists!" 336 mBodyNameMap[body_name] = mBodies.size() - 1;
344 unsigned int prev_joint_index = mJoints.size() - 1;
345 mJoints.push_back(joint);
348 mJoints[mJoints.size() - 1].q_index =
349 mJoints[prev_joint_index].q_index + mJoints[prev_joint_index].mDoFCount;
351 mJoints[mJoints.size() - 1].q_index =
352 mJoints[prev_joint_index].q_index + mJoints[prev_joint_index].mDoFCount;
362 multdof3_S.push_back(Matrix63::Zero(6, 3));
363 multdof3_U.push_back(Matrix63::Zero());
364 multdof3_Dinv.push_back(Matrix3d::Zero());
365 multdof3_u.push_back(Vector3d::Zero());
366 multdof3_w_index.push_back(0);
372 int multdof3_joint_counter = 0;
374 for (
unsigned int i = 1; i < mJoints.size(); i++) {
376 multdof3_w_index[i] = dof_count + multdof3_joint_counter;
377 multdof3_joint_counter++;
381 q_size = dof_count + multdof3_joint_counter;
387 X_T.push_back(joint_frame * movable_parent_transform);
391 IA.push_back(SpatialMatrix::Zero(6, 6));
395 d = VectorNd::Zero(mBodies.size());
396 u = VectorNd::Zero(mBodies.size());
411 if (mBodies.size() == fixed_body_discriminator) {
412 std::cerr <<
"Error: cannot add more than " << fixed_body_discriminator
413 <<
" movable bodies. You need to modify " 414 "Model::fixed_body_discriminator for this." 420 previously_added_body_id = mBodies.size() - 1;
422 mJointUpdateOrder.clear();
425 std::vector<std::pair<JointType, unsigned int>> joint_types;
426 for (
unsigned int i = 0; i < mJoints.size(); i++) {
427 joint_types.push_back(
428 std::pair<JointType, unsigned int>(mJoints[i].mJointType, i));
429 mJointUpdateOrder.push_back(mJoints[i].mJointType);
432 mJointUpdateOrder.clear();
434 while (joint_types.size() != 0) {
435 current_joint_type = joint_types[0].first;
437 std::vector<std::pair<JointType, unsigned int>>::iterator type_iter =
440 while (type_iter != joint_types.end()) {
441 if (type_iter->first == current_joint_type) {
442 mJointUpdateOrder.push_back(type_iter->second);
443 type_iter = joint_types.erase(type_iter);
456 return previously_added_body_id;
463 std::string body_name) {
465 previously_added_body_id,
473 const unsigned int parent_id,
477 std::string body_name) {
483 mCustomJoints.push_back(custom_joint);
485 unsigned int body_id =
486 AddBody(parent_id, joint_frame, proxy_joint, body, body_name);
Compact representation of spatial transformations.
Compact representation for Spatial Inertia.
static SpatialRigidBodyInertia createFromMassComInertiaC(double mass, const Vector3d &com, const Matrix3d &inertia_C)
bool IsFixedBodyId(unsigned int body_id)
Checks whether the body is rigidly attached to another body.
Contains all information about the rigid body model.
SpatialVector_t SpatialVector
3 DoF joint that uses Euler ZXY convention (faster than emulated multi DoF joints).
std::vector< Body > mBodies
All bodies 0 ... N_B, including the base.
unsigned int AddBody(const unsigned int parent_id, const Math::SpatialTransform &joint_frame, const Joint &joint, const Body &body, std::string body_name="")
Connects a given body to the model.
Namespace for all structures of the RigidBodyDynamics library.
Fixed joint which causes the inertial properties to be merged with the parent body.
std::vector< Math::SpatialRigidBodyInertia > I
The spatial inertia of body i (used only in CompositeRigidBodyAlgorithm())
unsigned int mMovableParent
Id of the movable body that this fixed body is attached to.
JointType
General types of joints.
void Join(const Math::SpatialTransform &transform, const Body &other_body)
Joins inertial parameters of two bodies to create a composite body.
std::vector< FixedBody > mFixedBodies
All bodies that are attached to a body via a fixed joint.
3 DoF joint that uses Euler ZYX convention (faster than emulated multi DoF joints).
unsigned int AppendBody(const Math::SpatialTransform &joint_frame, const Joint &joint, const Body &body, std::string body_name="")
Adds a Body to the model such that the previously added Body is the Parent.
Math::Matrix3d mInertia
Inertia matrix at the center of mass.
3 DoF joint that uses Euler XYZ convention (faster than emulated multi DoF joints).
Math::Vector3d mCenterOfMass
The position of the center of mass in body coordinates.
unsigned int mDoFCount
Number of degrees of freedom of the joint. Note: CustomJoints.
Math types such as vectors and matrices and utility functions.
unsigned int AddBodyCustomJoint(const unsigned int parent_id, const Math::SpatialTransform &joint_frame, CustomJoint *custom_joint, const Body &body, std::string body_name="")
Math::SpatialTransform mParentTransform
Transforms spatial quantities expressed for the parent to the.
Keeps the information of a body and how it is attached to another body.
A 6-DoF joint for floating-base (or freeflyer) systems.
unsigned int fixed_body_discriminator
Value that is used to discriminate between fixed and movable bodies.
double mMass
The mass of the body.
static FixedBody CreateFromBody(const Body &body)
JointType mJointType
Type of joint.
3 DoF joint using Quaternions for joint positional variables and angular velocity for joint velocity ...
Describes a joint relative to the predecessor body.
3 DoF joint that uses Euler YXZ convention (faster than emulated multi DoF joints).
std::map< std::string, unsigned int > mBodyNameMap
Human readable names for the bodies.
Math::SpatialVector * mJointAxes
The spatial axes of the joint.
Describes all properties of a single body.
User defined joints of varying size.