Added load functions #26
Conversation
…-sai-org/sai2-model into dev/singularity_functions
examples/06-add_load/main.cpp
Outdated
| @@ -0,0 +1,129 @@ | |||
| // 02-update_model: | |||
There was a problem hiding this comment.
update or remove those comments
examples/06-add_load/main.cpp
Outdated
| robot_with_load->setQ(q); | ||
| robot_with_load->updateModel(); | ||
|
|
||
| // tests |
There was a problem hiding this comment.
This should be an example, not a test. In the example, you want to show how to use the functions related to this feature: how to add a load and how to remove the load, maybe adding several loads and showing the effect on the model. But a single model to which you add a load and then remove it and compare the gravity and mass matrix by printing them for example is good.
And then you want to make a unit test case (or several if needed) in the tests/Tests_Sai2Model.cpp file where you can make the comparisons you make here
| RigidBodyDynamics::Body body = RigidBodyDynamics::Body(-load_params.mass, \ | ||
| RigidBodyDynamics::Math::Vector3d(load_params.com_pos), \ | ||
| RigidBodyDynamics::Math::Matrix3d(-load_params.inertia)); | ||
| // erase existing body, add body with same name, and remove body again |
There was a problem hiding this comment.
Why do we need to do all that ? It seems weird to have to add a body with negative mass to the model. Can't we just erase the body ?
| * @param inertia load inertia measured about the parent joint frame | ||
| * @param body_name unique body name for identifier | ||
| */ | ||
| void addLoad(const std::string& link_name, |
There was a problem hiding this comment.
Can we use a Transform3d to set the position and orientation of the fixed body in parent instead of just the position ? That way we can use the inseria of the added body in its own inertial frame which is easier to use in general
| const Vector3d& com_pos, | ||
| const Matrix3d& inertia, | ||
| const std::string& body_name) { | ||
| if (_load_names_to_load_mass_map.find(body_name) != _load_names_to_load_mass_map.end()) { |
There was a problem hiding this comment.
Also add a check to make sure that the link name exists, and other ones to make sure the mass and inertia are physically valid (positive mass and positive definite inertia)
| const double& mass, | ||
| const Vector3d& com_pos, | ||
| const Matrix3d& inertia, | ||
| const std::string& body_name) { |
There was a problem hiding this comment.
Call that load_name instead of body_name. It is cleaner
| _load_names_to_load_mass_map.insert(std::make_pair(body_name, LinkMassParams(mass, com_pos, inertia, link_name))); | ||
| } | ||
|
|
||
| void Sai2Model::Sai2Model::removeLoad(const std::string body_name) { |
There was a problem hiding this comment.
load_name instead of body_name to be consistent with the function name (removeLoad)
Functions for adding/removing loads at a link.