Skip to content

Commit

Permalink
[core] Add armature-like flexibility inertia parameter to fix numeric…
Browse files Browse the repository at this point in the history 
…al stability issues.
  • Loading branch information
Alexis Duburcq committed Oct 29, 2021
1 parent 0b59909 commit 17675af
Show file tree
Hide file tree
Showing 9 changed files with 117 additions and 96 deletions.
19 changes: 12 additions & 7 deletions core/include/jiminy/core/Types.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -127,23 +127,27 @@ namespace jiminy
struct flexibleJointData_t
{
std::string frameName;
vectorN_t stiffness;
vectorN_t damping;
vector3_t stiffness;
vector3_t damping;
vector3_t inertia;

flexibleJointData_t(void) :
frameName(),
stiffness(),
damping()
damping(),
inertia()
{
// Empty.
};

flexibleJointData_t(std::string const & frameNameIn,
vectorN_t const & stiffnessIn,
vectorN_t const & dampingIn) :
vector3_t const & stiffnessIn,
vector3_t const & dampingIn,
vector3_t const & inertiaIn) :
frameName(frameNameIn),
stiffness(stiffnessIn),
damping(dampingIn)
damping(dampingIn),
inertia(inertiaIn)
{
// Empty.
};
Expand All @@ -152,7 +156,8 @@ namespace jiminy
{
return (this->frameName == other.frameName
&& this->stiffness == other.stiffness
&& this->damping == other.damping);
&& this->damping == other.damping
&& this->inertia == other.inertia);
};
};

Expand Down
2 changes: 1 addition & 1 deletion core/include/jiminy/core/robot/Model.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -392,7 +392,7 @@ namespace jiminy
hresult_t removeConstraints(std::vector<std::string> const & constraintsNames,
constraintsHolderType_t const & holderType);

hresult_t refreshCollisionsProxies(void);
hresult_t refreshGeometryProxies(void);
hresult_t refreshContactsProxies(void);
/// \brief Refresh the proxies of the kinematics constraints.
hresult_t refreshConstraintsProxies(void);
Expand Down
36 changes: 32 additions & 4 deletions core/include/jiminy/core/utilities/Json.tpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,11 +15,39 @@ namespace jiminy
return {value};
}

template<>
Json::Value convertToJson<vectorN_t>(vectorN_t const & value);
template<typename T, int RowsAtCompileTime>
Json::Value convertToJson(Eigen::Matrix<T, RowsAtCompileTime, 1> const & value)
{
Json::Value row(Json::arrayValue);
for (Eigen::Index i = 0; i < value.size(); ++i)
{
row.append(value[i]);
}
return row;
}

template<>
Json::Value convertToJson<matrixN_t>(matrixN_t const & value);
template<typename T>
Json::Value convertToJson(Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> const & value)
{
Json::Value mat(Json::arrayValue);
if (value.rows() > 0)
{
for (Eigen::Index i = 0; i<value.rows(); ++i)
{
Json::Value row(Json::arrayValue);
for (Eigen::Index j = 0; j<value.cols(); ++j)
{
row.append(value(i, j));
}
mat.append(row);
}
}
else
{
mat.append(Json::Value(Json::arrayValue));
}
return mat;
}

template<>
Json::Value convertToJson<flexibleJointData_t>(flexibleJointData_t const & value);
Expand Down
73 changes: 48 additions & 25 deletions core/src/robot/Model.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -272,7 +272,7 @@ namespace jiminy
visualModelOrig_ = visualModel;

// Add ground geometry object to collision model is not already available
if (!collisionModel_.existGeometryName("ground"))
if (!collisionModelOrig_.existGeometryName("ground"))
{
// Instantiate ground FCL box geometry, wrapped as a pinocchio collision geometry.
// Note that half-space cannot be used for Shape-Shape collision because it has no
Expand All @@ -288,7 +288,7 @@ namespace jiminy
pinocchio::GeometryObject groundPlane("ground", 0, 0, groudBox, groundPose);

// Add the ground plane pinocchio to the robot model
collisionModel_.addGeometryObject(groundPlane, pncModelOrig_);
collisionModelOrig_.addGeometryObject(groundPlane, pncModelOrig_);
}

// Backup the original model and data
Expand Down Expand Up @@ -526,7 +526,7 @@ namespace jiminy
return hresult_t::SUCCESS; // Nothing to do. Returning early.
}

if (collisionModel_.ngeoms == 0) // If successfully loaded, the ground should be available
if (collisionModelOrig_.ngeoms == 0) // If successfully loaded, the ground should be available
{
PRINT_ERROR("Collision geometry not available. Some collision meshes were probably not found.");
return hresult_t::ERROR_INIT_FAILED;
Expand Down Expand Up @@ -560,7 +560,7 @@ namespace jiminy
for (std::string const & name : bodyNames)
{
bool_t hasGeometry = false;
for (pinocchio::GeometryObject const & geom : collisionModel_.geometryObjects)
for (pinocchio::GeometryObject const & geom : collisionModelOrig_.geometryObjects)
{
bool_t const isGeomMesh = (geom.meshPath.find('/') != std::string::npos ||
geom.meshPath.find('\\') != std::string::npos);
Expand All @@ -582,14 +582,14 @@ namespace jiminy
collisionBodiesNames_.insert(collisionBodiesNames_.end(), bodyNames.begin(), bodyNames.end());

// Create the collision pairs and add them to the geometry model of the robot
pinocchio::GeomIndex const & groundId = collisionModel_.getGeometryId("ground");
pinocchio::GeomIndex const & groundId = collisionModelOrig_.getGeometryId("ground");
for (std::string const & name : bodyNames)
{
// Find the geometries having the body for parent, and add a collision pair for each of them
constraintsMap_t collisionConstraintsMap;
for (std::size_t i = 0; i < collisionModel_.geometryObjects.size(); ++i)
for (std::size_t i = 0; i < collisionModelOrig_.geometryObjects.size(); ++i)
{
pinocchio::GeometryObject const & geom = collisionModel_.geometryObjects[i];
pinocchio::GeometryObject const & geom = collisionModelOrig_.geometryObjects[i];
bool_t const isGeomMesh = (geom.meshPath.find('/') != std::string::npos ||
geom.meshPath.find('\\') != std::string::npos);
std::string const & frameName = pncModel_.frames[geom.parentFrame].name;
Expand All @@ -599,7 +599,7 @@ namespace jiminy
Note that the ground always comes second for the normal to be
consistently compute wrt the ground instead of the body. */
pinocchio::CollisionPair const collisionPair(i, groundId);
collisionModel_.addCollisionPair(collisionPair);
collisionModelOrig_.addCollisionPair(collisionPair);

if (returnCode == hresult_t::SUCCESS)
{
Expand Down Expand Up @@ -636,11 +636,10 @@ namespace jiminy
}
}


// Refresh proxies associated with the collisions only
if (returnCode == hresult_t::SUCCESS)
{
refreshCollisionsProxies();
refreshGeometryProxies();
}

return returnCode;
Expand Down Expand Up @@ -691,18 +690,18 @@ namespace jiminy

// Get the indices of the corresponding collision pairs in the geometry model of the robot and remove them
std::vector<std::string> collisionConstraintsNames;
pinocchio::GeomIndex const & groundId = collisionModel_.getGeometryId("ground");
pinocchio::GeomIndex const & groundId = collisionModelOrig_.getGeometryId("ground");
for (std::string const & name : bodyNames)
{
// Find the geometries having the body for parent, and remove the collision pair for each of them
for (std::size_t i = 0; i < collisionModel_.geometryObjects.size(); ++i)
for (std::size_t i = 0; i < collisionModelOrig_.geometryObjects.size(); ++i)
{
pinocchio::GeometryObject const & geom = collisionModel_.geometryObjects[i];
pinocchio::GeometryObject const & geom = collisionModelOrig_.geometryObjects[i];
if (pncModel_.frames[geom.parentFrame].name == name)
{
// Remove the collision pair with the ground
pinocchio::CollisionPair const collisionPair(i, groundId);
collisionModel_.removeCollisionPair(collisionPair);
collisionModelOrig_.removeCollisionPair(collisionPair);

// Append collision geometry to the list of constraints to remove
if (constraintsHolder_.exist(geom.name, constraintsHolderType_t::COLLISION_BODIES))
Expand All @@ -718,7 +717,7 @@ namespace jiminy
removeFrames(collisionConstraintsNames);

// Refresh proxies associated with the collisions only
refreshCollisionsProxies();
refreshGeometryProxies();

return hresult_t::SUCCESS;
}
Expand Down Expand Up @@ -1071,6 +1070,16 @@ namespace jiminy
flexibleJointsNames_.emplace_back(flexName);
}

// Add flexibility armuture-like inertia to the model
for(flexibleJointData_t const & flexibleJoint : mdlOptions_->dynamics.flexibilityConfig)
{
std::string const & frameName = flexibleJoint.frameName;
std::string flexName = frameName + FLEXIBLE_JOINT_SUFFIX;
int32_t jointVelocityIdx;
::jiminy::getJointVelocityIdx(pncModelFlexibleOrig_, flexName, jointVelocityIdx);
pncModelFlexibleOrig_.rotorInertia.segment<3>(jointVelocityIdx) = flexibleJoint.inertia;
}

// Compute flexible joint indices
getJointsModelIdx(pncModelFlexibleOrig_, flexibleJointsNames_, flexibleJointsModelIdx_);

Expand Down Expand Up @@ -1430,7 +1439,7 @@ namespace jiminy

if (returnCode == hresult_t::SUCCESS)
{
returnCode = refreshCollisionsProxies();
returnCode = refreshGeometryProxies();
}

if (returnCode == hresult_t::SUCCESS)
Expand All @@ -1446,7 +1455,7 @@ namespace jiminy
return returnCode;
}

hresult_t Model::refreshCollisionsProxies(void)
hresult_t Model::refreshGeometryProxies(void)
{
hresult_t returnCode = hresult_t::SUCCESS;

Expand All @@ -1458,6 +1467,23 @@ namespace jiminy

if (returnCode == hresult_t::SUCCESS)
{
// Restore collision and visual models
collisionModel_ = collisionModelOrig_;
visualModel_ = visualModelOrig_;

// Update joint/frame fix for every geometry objects
if (mdlOptions_->dynamics.enableFlexibleModel)
{
for (auto model : std::array<pinocchio::GeometryModel *, 2>{{&collisionModel_, &visualModel_}})
{
for (pinocchio::GeometryObject & geom : model->geometryObjects)
{
geom.parentFrame = pncModel_.getFrameId(pncModelOrig_.frames[geom.parentFrame].name);
geom.parentJoint = pncModel_.getJointId(pncModelOrig_.names[geom.parentJoint]);
}
}
}

// Update geometry data object after changing the collision pairs
if (collisionData_.get())
{
Expand All @@ -1466,14 +1492,11 @@ namespace jiminy
}
else
{
/* Use copy assignment to avoid changing memory pointers, to
avoid dangling reference at Python-side. */
/* Use copy assignment to avoid changing memory pointers, which would
result in dangling reference at Python-side. */
collisionData_ = std::make_unique<pinocchio::GeometryData>(collisionModel_);
}
pinocchio::updateGeometryPlacements(pncModel_,
pncData_,
collisionModel_,
*collisionData_);
pinocchio::updateGeometryPlacements(pncModel_, pncData_, collisionModel_, *collisionData_);

// Set the max number of contact points per collision pairs
for (hpp::fcl::CollisionRequest & collisionRequest : collisionData_->collisionRequests)
Expand Down Expand Up @@ -1722,8 +1745,8 @@ namespace jiminy
}
else if (isCollisionDataInvalid)
{
// Update the collision data
refreshCollisionsProxies();
// Update the visual and collision data
refreshGeometryProxies();
}

return hresult_t::SUCCESS;
Expand Down
38 changes: 3 additions & 35 deletions core/src/utilities/Json.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,47 +13,14 @@ namespace jiminy
{
// *************** Convertion to JSON utilities *****************

template<>
Json::Value convertToJson<vectorN_t>(vectorN_t const & value)
{
Json::Value row(Json::arrayValue);
for (Eigen::Index i = 0; i < value.size(); ++i)
{
row.append(value[i]);
}
return row;
}

template<>
Json::Value convertToJson<matrixN_t>(matrixN_t const & value)
{
Json::Value mat(Json::arrayValue);
if (value.rows() > 0)
{
for (Eigen::Index i = 0; i<value.rows(); ++i)
{
Json::Value row(Json::arrayValue);
for (Eigen::Index j = 0; j<value.cols(); ++j)
{
row.append(value(i, j));
}
mat.append(row);
}
}
else
{
mat.append(Json::Value(Json::arrayValue));
}
return mat;
}

template<>
Json::Value convertToJson<flexibleJointData_t>(flexibleJointData_t const & value)
{
Json::Value flex;
flex["frameName"] = convertToJson(value.frameName);
flex["stiffness"] = convertToJson(value.stiffness);
flex["damping"] = convertToJson(value.damping);
flex["inertia"] = convertToJson(value.inertia);
return flex;
}

Expand Down Expand Up @@ -196,7 +163,8 @@ namespace jiminy
return {
convertFromJson<std::string>(value["frameName"]),
convertFromJson<vectorN_t>(value["stiffness"]),
convertFromJson<vectorN_t>(value["damping"])
convertFromJson<vectorN_t>(value["damping"]),
convertFromJson<vectorN_t>(value["inertia"])
};
}

Expand Down
8 changes: 4 additions & 4 deletions core/src/utilities/Pinocchio.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -691,12 +691,12 @@ namespace jiminy
}

/* Add weightless body.
In practice having a zero inertia makes some of pinocchio algorithm crash,
so we set a very small value instead: 1.0g. Anything below that creates
numerical instability. */
In practice having a zero inertia makes some of pinocchio algorithm
crash, so we set a very small value instead: 1g. Anything below
creates numerical instability. */
float64_t const mass = 1.0e-3;
float64_t const lengthSemiAxis = 1.0;
pinocchio::Inertia inertia = pinocchio::Inertia::FromEllipsoid(
pinocchio::Inertia const inertia = pinocchio::Inertia::FromEllipsoid(
mass, lengthSemiAxis, lengthSemiAxis, lengthSemiAxis);

modelInOut.appendBodyToJoint(newJointIdx, inertia, SE3::Identity());
Expand Down
Loading

0 comments on commit 17675af

Please sign in to comment.