Commit 26491894 authored by Jordan Lack's avatar Jordan Lack

Merge branch 'feature/RDL-110' into 'develop'

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

See merge request !60
parents a4662deb 34fffad9
Pipeline #55804804 passed with stages
in 50 minutes and 24 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