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 abd08cd
Show file tree
Hide file tree
Showing 7 changed files with 92 additions and 70 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
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
25 changes: 25 additions & 0 deletions core/src/robot/Model.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1071,6 +1071,16 @@ namespace jiminy
flexibleJointsNames_.emplace_back(flexName);
}

// Add flexibility 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 @@ -1161,6 +1171,21 @@ namespace jiminy
pinocchio::centerOfMass(pncModel_, pncData_,
pinocchio::neutral(pncModel_));

// Update joint/frame fix for every geometry objects of both collision and visual models
collisionModel_ = collisionModelOrig_;
visualModel_ = visualModelOrig_;
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]);
}
}
}

// Refresh internal proxies
returnCode = refreshProxies();
}
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
10 changes: 5 additions & 5 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. */
float64_t const mass = 1.0e-3;
In practice having a zero inertia makes some of pinocchio algorithm
crash, so we set a very small value instead: 0.1g. Anything below
creates numerical instability. */
float64_t const mass = 1.0e-4;
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
32 changes: 13 additions & 19 deletions python/jiminy_py/src/jiminy_py/viewer/viewer.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -721,7 +721,7 @@ def get_dcm_scale() -> Tuple3FType:
# Add contact frame markers
for frame_name, frame_idx in zip(
robot.contact_frames_names, robot.contact_frames_idx):
frame_pose = robot.pinocchio_data.oMf[frame_idx]
frame_pose = self._client.data.oMf[frame_idx]
self.add_marker(name='_'.join(("ContactFrame", frame_name)),
shape="sphere",
color="yellow",
Expand Down Expand Up @@ -764,31 +764,25 @@ def get_force_pose(
joint_idx: int) -> Tuple[Tuple3FType, Tuple4FType]:
joint_pose = self._client.data.oMi[joint_idx]
joint_position = joint_pose.translation
if self.f_external:
f_ext = self.f_external[joint_idx - 1].linear
joint_rotation = pin.Quaternion(joint_pose.rotation)
frame_rotation = (joint_rotation * pin.Quaternion(
np.array([0.0, 0.0, 1.0]), f_ext)).coeffs()
else:
frame_rotation = pin.Quaternion.Identity().coeffs()
f_ext = self.f_external[joint_idx - 1].linear
joint_rotation = pin.Quaternion(joint_pose.rotation)
frame_rotation = (joint_rotation * pin.Quaternion(
np.array([0.0, 0.0, 1.0]), f_ext)).coeffs()
return (joint_position, frame_rotation)

def get_force_scale(joint_idx: int) -> Tuple[float, float, float]:
if self.f_external:
f_ext = self.f_external[joint_idx - 1].linear
f_ext_norm = np.linalg.norm(f_ext, 2)
length = min(f_ext_norm / EXTERNAL_FORCE_SCALE, 1.0)
else:
length = 0.0
f_ext = self.f_external[joint_idx - 1].linear
f_ext_norm = np.linalg.norm(f_ext, 2)
length = min(f_ext_norm / EXTERNAL_FORCE_SCALE, 1.0)
return (1.0, 1.0, length)

for i in range(1, pinocchio_model.njoints):
frame_name = self._client.model.names[i]
self.add_marker(name=f"ForceExternal_{frame_name}",
for joint_name in pinocchio_model.names[1:]:
joint_idx = self._client.model.getJointId(joint_name)
self.add_marker(name=f"ForceExternal_{joint_name}",
shape="arrow",
color="red",
pose=partial(get_force_pose, i),
scale=partial(get_force_scale, i),
pose=partial(get_force_pose, joint_idx),
scale=partial(get_force_scale, joint_idx),
remove_if_exists=True,
auto_refresh=False,
radius=0.015,
Expand Down
2 changes: 2 additions & 0 deletions python/jiminy_pywrap/include/jiminy/python/Utilities.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -316,6 +316,7 @@ namespace python
flexibilityJointDataPy["frameName"] = flexibleJointData.frameName;
flexibilityJointDataPy["stiffness"] = flexibleJointData.stiffness;
flexibilityJointDataPy["damping"] = flexibleJointData.damping;
flexibilityJointDataPy["inertia"] = flexibleJointData.inertia;
return flexibilityJointDataPy;
}

Expand Down Expand Up @@ -606,6 +607,7 @@ namespace python
flexData.frameName = convertFromPython<std::string>(flexDataPy["frameName"]);
flexData.stiffness = convertFromPython<vectorN_t>(flexDataPy["stiffness"]);
flexData.damping = convertFromPython<vectorN_t>(flexDataPy["damping"]);
flexData.inertia = convertFromPython<vectorN_t>(flexDataPy["inertia"]);
return flexData;
}

Expand Down

0 comments on commit abd08cd

Please sign in to comment.