Commit 34fffad9 authored by Jordan Lack's avatar Jordan Lack

add mass member var to Model object and add body mass to it as bodies are...

add mass member var to Model object and add body mass to it as bodies are added. Also add some tests
parent a4662deb
Pipeline #55619722 passed with stages
in 58 minutes and 56 seconds
......@@ -39,4 +39,7 @@ Fix issues making rdl incompatible with melodic
Removed a bunch of deprecated functions
--- 04-06-2019 ---
Reworked FrameOrientation inheritance so it inherits from Quaternion instead of having a Quaternion
\ No newline at end of file
Reworked FrameOrientation inheritance so it inherits from Quaternion instead of having a Quaternion
--- 04-07-2019 ---
Added Model::mass variable where you can get the mass of the model
\ No newline at end of file
......@@ -170,6 +170,9 @@ struct RDL_DLLAPI Model
*/
unsigned int qdot_size;
/**< total robot mass(kg) */
double mass;
/// @brief Id of the previously added body, required for Model::appendBody()
unsigned int previously_added_body_id;
......
......@@ -25,6 +25,8 @@ Model::Model()
Body root_body;
Joint root_joint;
mass = 0.;
// structural information
lambda.push_back(0);
lambda_q.push_back(0);
......@@ -97,6 +99,7 @@ unsigned int addBodyFixedJoint(Model& model, const unsigned int parent_id, const
std::string body_name)
{
FixedBody fbody = FixedBody::CreateFromBody(body);
model.mass += fbody.mMass;
fbody.mMovableParent = parent_id;
fbody.mParentTransform = joint_frame;
......@@ -489,6 +492,7 @@ unsigned int Model::addBody(const unsigned int parent_id, const SpatialTransform
parent_chain[parent_chain.size() - 1] = previously_added_body_id; // add this joint
lambda_chain.push_back(parent_chain);
mass += body.mMass;
return previously_added_body_id;
}
......
......@@ -61,6 +61,7 @@ TEST_F(RdlModelFixture, testCommonParentId)
EXPECT_EQ(model->getCommonMovableParentId(id4, id1), id1);
EXPECT_EQ(model->getCommonMovableParentId(0, id8), 0);
EXPECT_EQ(model->getCommonMovableParentId(id4, id4), id4);
EXPECT_EQ(model->mass, 8.0);
unsigned int id9 = model->addBody(0, SpatialTransform(), j, b, "b9");
unsigned int id10 = model->addBody(id9, SpatialTransform(), j, b, "b10");
......@@ -68,12 +69,16 @@ TEST_F(RdlModelFixture, testCommonParentId)
EXPECT_EQ(model->getCommonMovableParentId(id10, id8), 0);
EXPECT_EQ(model->getCommonMovableParentId(id8, id10), 0);
EXPECT_EQ(model->mass, 10.0);
Joint jf(JointTypeFixed);
unsigned int fid1 = model->addBody(id10, SpatialTransform(), jf, b, "fb1");
unsigned int fid2 = model->addBody(fid1, SpatialTransform(), jf, b, "fb2");
unsigned int fid3 = model->addBody(id2, SpatialTransform(), jf, b, "fb3");
unsigned int fid4 = model->addBody(id10, SpatialTransform(), jf, b, "fb4");
EXPECT_EQ(model->mass, 14.0);
EXPECT_EQ(model->getCommonMovableParentId(fid1, fid2), id10);
EXPECT_EQ(model->getCommonMovableParentId(fid2, fid1), id10);
EXPECT_EQ(model->getCommonMovableParentId(fid1, fid4), id10);
......@@ -403,8 +408,8 @@ TEST_F(RdlModelFixture, Model3DoFJoint)
model_std.addBody(body_id, Xtrans(Vector3d(1., 0., 0.)), joint_rot_z, null_body);
model_std.appendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_y, null_body);
body_id = model_std.appendBody(Xtrans(Vector3d(0., 0., 0.)), joint_rot_x, body, "body2");
// using a model with a 2 DoF joint
EXPECT_EQ(model_std.mass, 2.0);
// using a model with a 3 DoF joint
Joint joint_rot_zyx(SpatialVector(0., 0., 1., 0., 0., 0.), SpatialVector(0., 1., 0., 0., 0., 0.), SpatialVector(1., 0., 0., 0., 0., 0.));
Model model_2;
......@@ -414,6 +419,7 @@ TEST_F(RdlModelFixture, Model3DoFJoint)
// correct.
body_id = model_2.addBody(0, Xtrans(Vector3d(1., 0., 0.)), joint_rot_zyx, body);
body_id = model_2.addBody(body_id, Xtrans(Vector3d(1., 0., 0.)), joint_rot_zyx, body);
EXPECT_EQ(model_2.mass, 2.0);
VectorNd Q = VectorNd::Zero(model_std.dof_count);
VectorNd QDot = VectorNd::Zero(model_std.dof_count);
......
......@@ -117,6 +117,7 @@ TEST_F(RdlUtilsTests, TestCOMSimple)
Utils::calcCenterOfMass(model, q, pcom_2);
EXPECT_EQ(123., mass);
EXPECT_EQ(model.mass, 123.);
EXPECT_TRUE(unit_test_utils::checkVector3dEq(Vector3d(0., 0., 0.), com));
EXPECT_TRUE(unit_test_utils::checkVector3dEq(pcom_2.vec(), com));
EXPECT_TRUE(unit_test_utils::checkVector3dEq(p_com.vec(), Vector3d(0., 0., 0.)));
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment