Skip to content

Commit

Permalink
[core] Fix internal efforts computation. (#684)
Browse files Browse the repository at this point in the history 
* [core] Fix wrong state and derivative during computation of extra terms.
* [core] Fix internal effort computation.
* [python/viewer] Fix exception handling at init.
* [python/simulator] Fix exception when manually closing viewer.
* [misc] Update MacOS CI.
  • Loading branch information
@duburcqa
duburcqa committed Jan 2, 2024
1 parent eb8e39e commit 3b02f79
Show file tree
Hide file tree
Showing 6 changed files with 120 additions and 89 deletions.
15 changes: 9 additions & 6 deletions .github/workflows/macos.yml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@ jobs:
name: >-
(${{ matrix.PYTHON_VERSION }}) (${{ matrix.BUILD_TYPE }})
Build and run the unit tests. Then generate and publish the wheels on PyPi.
runs-on: macos-11
runs-on: macos-13

strategy:
matrix:
Expand All @@ -30,8 +30,8 @@ jobs:
TORCH_VERSION: "2.0.1"
CMAKE_C_COMPILER: "/usr/bin/clang"
CMAKE_CXX_COMPILER: "/usr/bin/clang++"
CMAKE_CXX_FLAGS: "-DEIGEN_MPL2_ONLY -Wno-deprecated-declarations"
OSX_DEPLOYMENT_TARGET: "10.15"
CMAKE_CXX_FLAGS: "-DEIGEN_MPL2_ONLY -D_LIBCPP_ENABLE_CXX17_REMOVED_UNARY_BINARY_FUNCTION -Wno-deprecated-declarations"
OSX_DEPLOYMENT_TARGET: "11.0"
OSX_ARCHITECTURES: "x86_64;arm64"
WHEEL_ARCH: "universal2"

Expand Down Expand Up @@ -108,8 +108,8 @@ jobs:

- name: Generate and install Python Pip wheels
run: |
export LD_LIBRARY_PATH="${InstallDir}/lib/:/usr/local/lib"
export DYLD_FALLBACK_LIBRARY_PATH="$LD_LIBRARY_PATH"
export LD_LIBRARY_PATH="${InstallDir}/lib:${DYLD_LIBRARY_PATH}"
export DYLD_FALLBACK_LIBRARY_PATH="${LD_LIBRARY_PATH}"
# Generate stubs
stubgen -p jiminy_py -o ${RootDir}/build/pypi/jiminy_py/src
Expand Down Expand Up @@ -139,7 +139,8 @@ jobs:

- name: Build extension module
run: |
export LD_LIBRARY_PATH="${InstallDir}/lib/:/usr/local/lib"
export DYLD_FALLBACK_LIBRARY_PATH="${InstallDir}/lib"
"${InstallDir}/bin/jiminy_double_pendulum"
mkdir -p "${RootDir}/core/examples/external_project/build"
Expand All @@ -156,6 +157,8 @@ jobs:

- name: Run unit tests for jiminy
run: |
export DYLD_FALLBACK_LIBRARY_PATH="${InstallDir}/lib"
cd "${RootDir}/build/core/unit"
ctest --output-on-failure
Expand Down
46 changes: 0 additions & 46 deletions core/include/jiminy/core/robot/pinocchio_overload_algorithms.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -478,52 +478,6 @@ namespace jiminy::pinocchio_overload
return data.ddq;
}

template<typename TangentVectorType>
struct ForwardKinematicsAccelerationStep :
public pinocchio::fusion::JointUnaryVisitorBase<
ForwardKinematicsAccelerationStep<TangentVectorType>>
{
typedef boost::fusion::vector<const pinocchio::Model &,
pinocchio::Data &,
const Eigen::MatrixBase<TangentVectorType> &>
ArgsType;

template<typename JointModel>
static void algo(const pinocchio::JointModelBase<JointModel> & jmodel,
pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata,
const pinocchio::Model & model,
pinocchio::Data & data,
const Eigen::MatrixBase<TangentVectorType> & a)
{
pinocchio::JointIndex i = jmodel.id();
pinocchio::JointIndex parent = model.parents[i];
data.a[i] =
jdata.S() * jmodel.jointVelocitySelector(a) + jdata.c() + (data.v[i] ^ jdata.v());
if (parent > 0)
{
data.a[i] += data.liMi[i].actInv(data.a[parent]);
}
}
};

/// \brief Compute only joints spatial accelerations, assuming positions and velocities
/// are already up-to-date.
///
/// \warning This method does not update the internal buffer `data.ddq`. This buffer is only
/// updated by `aba` and `forwardDynamics` algorithms.
template<typename TangentVectorType>
inline void forwardKinematicsAcceleration(const pinocchio::Model & model,
pinocchio::Data & data,
const Eigen::MatrixBase<TangentVectorType> & a)
{
typedef ForwardKinematicsAccelerationStep<TangentVectorType> Pass;
data.a[0].setZero();
for (int32_t i = 1; i < model.njoints; ++i)
{
Pass::run(model.joints[i], data.joints[i], typename Pass::ArgsType(model, data, a));
}
}

template<typename JacobianType>
hresult_t computeJMinvJt(const pinocchio::Model & model,
pinocchio::Data & data,
Expand Down
117 changes: 85 additions & 32 deletions core/src/engine/engine_multi_robot.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -920,14 +920,32 @@ namespace jiminy
isTelemetryConfigured_ = false;
}

struct ForwardKinematicsAccelerationStep :
public pinocchio::fusion::JointUnaryVisitorBase<ForwardKinematicsAccelerationStep>
{
typedef boost::fusion::vector<pinocchio::Data &, const Eigen::VectorXd &> ArgsType;

template<typename JointModel>
static void algo(const pinocchio::JointModelBase<JointModel> & jmodel,
pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata,
pinocchio::Data & data,
const Eigen::VectorXd & a)
{
pinocchio::JointIndex i = jmodel.id();
data.a[i] = jdata.c() + data.v[i].cross(jdata.v());
data.a[i] += jdata.S() * jmodel.jointVelocitySelector(a);
}
};

/// \details This method is optimized to avoid redundant computations.
///
/// \see See `pinocchio::computeAllTerms` for reference:
/// https://github.com/stack-of-tasks/pinocchio/blob/a1df23c2f183d84febdc2099e5fbfdbd1fc8018b/src/algorithm/compute-all-terms.hxx
///
/// Copyright (c) 2014-2020, CNRS
/// Copyright (c) 2018-2020, INRIA
void computeExtraTerms(systemHolder_t & system, const systemDataHolder_t & systemData)
void computeExtraTerms(
systemHolder_t & system, const systemDataHolder_t & systemData, ForceVector & fExt)
{
const pinocchio::Model & model = system.robot->pncModel_;
pinocchio::Data & data = system.robot->pncData_;
Expand Down Expand Up @@ -964,25 +982,52 @@ namespace jiminy
accelerations, joint forces and body forces, so it must be done separately:
- 1st step: computing the accelerations based on ForwardKinematic algorithm
- 2nd step: computing the forces based on RNEA algorithm */
pinocchio_overload::forwardKinematicsAcceleration(model, data, data.ddq);

// Compute the spatial momenta and the sum of external forces acting on each body
/* Compute the true joint acceleration and the one due to the lone gravity field, then
the spatial momenta and the total internal and external forces acting on each body. */
data.h[0].setZero();
fExt[0].setZero();
data.f[0].setZero();
data.a[0].setZero();
data.a_gf[0] = -model.gravity;
for (int i = 1; i < model.njoints; ++i)
{
data.h[i] = model.inertias[i] * data.v[i];
data.f[i] = model.inertias[i] * data.a[i] + data.v[i].cross(data.h[i]);
const auto & jmodel = model.joints[i];
const pinocchio::JointIndex jointModelIdx = jmodel.id();
const pinocchio::JointIndex parentJointModelIdx = model.parents[jointModelIdx];

ForwardKinematicsAccelerationStep::run(
jmodel,
data.joints[i],
typename ForwardKinematicsAccelerationStep::ArgsType(data, systemData.state.a));
data.a_gf[jointModelIdx] = data.a[jointModelIdx];
data.a[jointModelIdx] += data.liMi[jointModelIdx].actInv(data.a[parentJointModelIdx]);
data.a_gf[jointModelIdx] +=
data.liMi[jointModelIdx].actInv(data.a_gf[parentJointModelIdx]);

model.inertias[jointModelIdx].__mult__(data.v[jointModelIdx], data.h[jointModelIdx]);

model.inertias[jointModelIdx].__mult__(data.a[jointModelIdx], fExt[jointModelIdx]);
data.f[jointModelIdx] = data.v[jointModelIdx].cross(data.h[jointModelIdx]);
fExt[jointModelIdx] += data.f[jointModelIdx];
data.f[jointModelIdx] += model.inertias[jointModelIdx] * data.a_gf[jointModelIdx];
data.f[jointModelIdx] -= systemData.state.fExternal[jointModelIdx];
}
for (int i = model.njoints - 1; i > 0; --i)
{
const pinocchio::JointIndex parentJointModelIdx = model.parents[i];
data.h[parentJointModelIdx] += data.liMi[i].act(data.h[i]);
data.f[parentJointModelIdx] += data.liMi[i].act(data.f[i]);
const auto & jmodel = model.joints[i];
const pinocchio::JointIndex jointModelIdx = jmodel.id();
const pinocchio::JointIndex parentJointModelIdx = model.parents[jointModelIdx];

fExt[parentJointModelIdx] += data.liMi[jointModelIdx].act(fExt[jointModelIdx]);
data.h[parentJointModelIdx] += data.liMi[jointModelIdx].act(data.h[jointModelIdx]);
if (parentJointModelIdx > 0)
{
data.f[parentJointModelIdx] += data.liMi[jointModelIdx].act(data.f[jointModelIdx]);
}
}

/* Now that `data.Ycrb` and `data.h` are available, one can get directly the position and
velocity of the center of mass of each subtrees. */
// Compute the position and velocity of the center of mass of each subtree
for (int i = 0; i < model.njoints; ++i)
{
data.com[i] = data.Ycrb[i].lever();
Expand All @@ -994,18 +1039,20 @@ namespace jiminy
// Compute centroidal dynamics and its derivative
data.hg = data.h[0];
data.hg.angular() += data.hg.linear().cross(data.com[0]);
data.dhg = data.f[0];
data.dhg = fExt[0];
data.dhg.angular() += data.dhg.linear().cross(data.com[0]);
}

void computeAllExtraTerms(std::vector<systemHolder_t> & systems,
const vector_aligned_t<systemDataHolder_t> & systemsDataHolder)
const vector_aligned_t<systemDataHolder_t> & systemsDataHolder,
vector_aligned_t<ForceVector> & f)
{
auto systemIt = systems.begin();
auto systemDataIt = systemsDataHolder.begin();
for (; systemIt != systems.end(); ++systemIt, ++systemDataIt)
auto fIt = f.begin();
for (; systemIt != systems.end(); ++systemIt, ++systemDataIt, ++fIt)
{
computeExtraTerms(*systemIt, *systemDataIt);
computeExtraTerms(*systemIt, *systemDataIt, *fIt);
}
}

Expand Down Expand Up @@ -1584,8 +1631,10 @@ namespace jiminy
systemIt->robot->setSensorsData(t, q, v, a, uMotor, fext);
}

// Compute joints accelerations and forces
computeAllExtraTerms(systems_, systemsDataHolder_);
// Compute all external terms including joints accelerations and forces if requested
computeAllExtraTerms(systems_, systemsDataHolder_, fPrev_);

// Backend the updated joint accelerations and forces
syncAllAccelerationsAndForces(systems_, contactForcesPrev_, fPrev_, aPrev_);

// Synchronize the global stepper state with the individual system states
Expand Down Expand Up @@ -2214,15 +2263,17 @@ namespace jiminy
// Reset successive iteration failure counter
successiveIterFailed = 0;

/* Compute the actual joint acceleration and forces.
Note that it is possible to call this method because pinocchio::Data is
guaranteed to be up-to-date at this point. */
computeAllExtraTerms(systems_, systemsDataHolder_);
// Synchronize the position, velocity and acceleration of all systems
syncSystemsStateWithStepper();

/* Compute all external terms including joints accelerations and forces.
Note that it is possible to call this method because `pinocchio::Data`
is guaranteed to be up-to-date at this point. */
computeAllExtraTerms(systems_, systemsDataHolder_, fPrev_);

// Synchronize the individual system states
// Backend the updated joint accelerations and forces
syncAllAccelerationsAndForces(
systems_, contactForcesPrev_, fPrev_, aPrev_);
syncSystemsStateWithStepper();

// Increment the iteration counter only for successful steps
++stepperState_.iter;
Expand Down Expand Up @@ -2338,13 +2389,15 @@ namespace jiminy
// Reset successive iteration failure counter
successiveIterFailed = 0;

// Compute the actual joint acceleration and forces
computeAllExtraTerms(systems_, systemsDataHolder_);
// Synchronize the position, velocity and acceleration of all systems
syncSystemsStateWithStepper();

// Synchronize the individual system states
// Compute all external terms including joints accelerations and forces
computeAllExtraTerms(systems_, systemsDataHolder_, fPrev_);

// Backend the updated joint accelerations and forces
syncAllAccelerationsAndForces(
systems_, contactForcesPrev_, fPrev_, aPrev_);
syncSystemsStateWithStepper();

// Increment the iteration counter
++stepperState_.iter;
Expand Down Expand Up @@ -3093,16 +3146,16 @@ namespace jiminy
pinocchio::forwardKinematics(model, data, q, v, a);

// Update frame placements (avoiding redundant computations)
for (int jointModelIdx = 1; jointModelIdx < model.nframes; ++jointModelIdx)
for (int frameIdx = 1; frameIdx < model.nframes; ++frameIdx)
{
const pinocchio::Frame & frame = model.frames[jointModelIdx];
const pinocchio::Frame & frame = model.frames[frameIdx];
pinocchio::JointIndex parentJointModelIdx = frame.parent;
switch (frame.type)
{
case pinocchio::FrameType::JOINT:
/* If the frame is associated with an actual joint, no need to compute anything
new, since the frame transform is supposed to be identity. */
data.oMf[jointModelIdx] = data.oMi[parentJointModelIdx];
data.oMf[frameIdx] = data.oMi[parentJointModelIdx];
break;
case pinocchio::FrameType::BODY:
if (model.frames[frame.previousFrame].type == pinocchio::FrameType::FIXED_JOINT)
Expand All @@ -3111,21 +3164,21 @@ namespace jiminy
itself, so no need to compute them twice. Here we are doing the assumption
that the previous frame transform has already been updated since it is
closer to root in kinematic tree. */
data.oMf[jointModelIdx] = data.oMf[frame.previousFrame];
data.oMf[frameIdx] = data.oMf[frame.previousFrame];
}
else
{
/* BODYs connected via JOINT(s) have the identity transform, so copying parent
joint transform should be fine. */
data.oMf[jointModelIdx] = data.oMi[parentJointModelIdx];
data.oMf[frameIdx] = data.oMi[parentJointModelIdx];
}
break;
case pinocchio::FrameType::FIXED_JOINT:
case pinocchio::FrameType::SENSOR:
case pinocchio::FrameType::OP_FRAME:
default:
// Nothing special, doing the actual computation
data.oMf[jointModelIdx] = data.oMi[parentJointModelIdx] * frame.placement;
data.oMf[frameIdx] = data.oMi[parentJointModelIdx] * frame.placement;
}
}

Expand Down
15 changes: 13 additions & 2 deletions core/src/robot/model.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1210,8 +1210,19 @@ namespace jiminy
Note that it will alter the actual joints spatial accelerations, so it is necessary to
do a backup first to restore it later on. */
jointsAcceleration_.swap(pncData_.a);
pinocchio_overload::forwardKinematicsAcceleration(
pncModel_, pncData_, Eigen::VectorXd::Zero(pncModel_.nv));
pncData_.a[0].setZero();
for (int i = 1; i < pncModel_.njoints; ++i)
{
const auto & jmodel = pncModel_.joints[i];
const auto & jdata = pncData_.joints[i];
const pinocchio::JointIndex jointModelIdx = jmodel.id();
const pinocchio::JointIndex parentJointModelIdx = pncModel_.parents[jointModelIdx];
pncData_.a[jointModelIdx] = jdata.c() + pncData_.v[jointModelIdx].cross(jdata.v());
if (parentJointModelIdx > 0)
{
pncData_.a[i] += pncData_.liMi[i].actInv(pncData_.a[parentJointModelIdx]);
}
}

// Compute sequentially the jacobian and drift of each enabled constraint
constraintsHolder_.foreach(
Expand Down
5 changes: 5 additions & 0 deletions python/jiminy_py/src/jiminy_py/simulator.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -490,6 +490,11 @@ def render(self,
# Instantiate the robot and viewer client if necessary.
# A new dedicated scene and window will be created.
if not self.is_viewer_available:
# Make sure that the current viewer is properly closed if any
if self.viewer is not None:
self.viewer.close()
self.viewer = None

# Create new viewer instance
self.viewer = Viewer(
self.robot,
Expand Down
11 changes: 8 additions & 3 deletions python/jiminy_py/src/jiminy_py/viewer/viewer.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -519,6 +519,11 @@ def __init__(self, # pylint: disable=unused-argument
Optional: Root joint for robot with freeflyer by default.
:param kwargs: Unused extra keyword arguments to enable forwarding.
"""
# Define some attribute to avoid raising exception at exit
self.robot_name = "_".join((
"undefined", next(tempfile._get_candidate_names())))
self.__is_open = False

# Make sure the robot is properly initialized
assert robot.is_initialized, (
"Robot not initialized. Impossible to instantiate a viewer.")
Expand Down Expand Up @@ -758,7 +763,7 @@ def get_com_scale() -> Tuple3FType:
remove_if_exists=True,
auto_refresh=False,
radius=0.004,
length=1.0,
length=-1.0,
anchor_bottom=True)

self.display_center_of_mass(self._display_com)
Expand Down Expand Up @@ -1119,8 +1124,8 @@ def close(self: Optional[Union["Viewer", Type["Viewer"]]] = None) -> None:
# Check if the backend process has changed or the viewer instance
# has already been closed, which may happened if it has been closed
# manually in the meantime. If so, there is nothing left to do.
if (Viewer._backend_proc is not self._backend_proc or
not self.__is_open):
if (not self.__is_open or
Viewer._backend_proc is not self._backend_proc):
return

# Assert(s) for type checker
Expand Down

0 comments on commit 3b02f79

Please sign in to comment.