/home/robot/ws_perceptive_mpc/src/ocs2/ocs2_core/src/integration/SensitivityIntegrator.cpp:76:75: required from here
/usr/include/c++/5/bits/hashtable_policy.h:85:34: error: no match for call to ‘(const std::hashocs2::SensitivityIntegratorType) (const ocs2::SensitivityIntegratorType&)’
noexcept(declval<const _Hash&>()(declval<const _Key&>()))>

I use cmake3.14.7 in ubuntu16.04 kinetic enviroment

Describe the bug
When testing the legged_robot in your examples by using full centroidal dynamics(FCD) method, I find that it will crash.

To Reproduce
Steps to reproduce the behavior:

1. build this project ocs2
2. source devel/setup.bash
3. change the config file in legged robot
   ocs2/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info
   I change the "centroidalModelType" to "0"
4. roslaunch ocs2_legged_robot legged_robot.launch

Expected behavior
This info i get
SUMMARY

PARAMETERS

• /legged_robot_description: <?xml version="1....
• /rosdistro: noetic
• /rosversion: 1.15.11

NODES
/
legged_robot_dummy (ocs2_legged_robot/legged_robot_dummy)
legged_robot_gait_command (ocs2_legged_robot/legged_robot_gait_command)
legged_robot_mpc (ocs2_legged_robot/legged_robot_mpc)
legged_robot_target (ocs2_legged_robot/legged_robot_target)
rviz (rviz/rviz)

auto-starting new master
process[master]: started with pid [41295]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to ffeea4d0-0256-11ec-ae58-db1db3bbff06
WARNING: Package name "JYPro" does not follow the naming conventions. It should start with a lower case letter and only contain lower case letters, digits, underscores, and dashes.
process[rosout-1]: started with pid [41324]
started core service [/rosout]
process[rviz-2]: started with pid [41328]
process[legged_robot_mpc-3]: started with pid [41332]
process[legged_robot_dummy-4]: started with pid [41333]
process[legged_robot_target-5]: started with pid [41335]
process[legged_robot_gait_command-6]: started with pid [41336]
[ WARN] [1629533323.482678414]: link 'imu_link' material 'orange' undefined.
[ WARN] [1629533323.484238724]: link 'imu_link' material 'orange' undefined.
Loading task file: /ocs2_ws/src/ocs2/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info

'display'................................................................0

Legged Robot Model Settings:

=============================================================================

'positionErrorGain'......................................................0

'phaseTransitionStanceTime'............................................0.4

'verboseCppAd'...........................................................1

'recompileLibrariesCppAd'................................................1

'modelFolderCppAd'.............................................../tmp/ocs2

=============================================================================

DDP Settings:

=============================================================================

'algorithm'............................................................SLQ

'nThreads'...............................................................3

'threadPriority'........................................................50

'maxNumIterations'......................................................10

'minRelCost'...........................................................0.1

'constraintTolerance'................................................0.005

'displayInfo'............................................................0

'displayShortSummary'....................................................0

'checkNumericalStability'................................................0

'debugPrintRollout'......................................................0

'debugCaching'...........................................................0

'AbsTolODE'..........................................................1e-05

'RelTolODE'..........................................................0.001

'maxNumStepsPerSecond'...............................................10000

'timeStep'............................................................0.01

'backwardPassIntegratorType'.........................................ODE45

'constraintPenaltyInitialValue'.........................................20

'constraintPenaltyIncreaseRate'..........................................2

'inequalityConstraintMu'...............................................0.1

'inequalityConstraintDelta'..............................................5

'preComputeRiccatiTerms'.................................................1

'useNominalTimeForBackwardPass'..........................................1

'useFeedbackPolicy'......................................................0

'riskSensitiveCoeff'.....................................................0 (default)

'strategy'.....................................................LINE_SEARCH

LINE_SEARCH Settings: {

'minStepLength'.....................................................0.0001

'maxStepLength'..........................................................1

'contractionRate'......................................................0.5 (default)

'armijoCoefficient'.................................................0.0001 (default)

'hessianCorrectionStrategy'.................................DIAGONAL_SHIFT

'hessianCorrectionMultiple'..........................................1e-05

}

=============================================================================

MPC Settings:

=============================================================================

'timeHorizon'............................................................1

'numPartitions'..........................................................2

'solutionTimeWindow'....................................................-1

'debugPrint'.............................................................0

'coldStart'..............................................................0

'initMaxNumIterations'..................................................10

'initMinStepLength'...................................................0.01

'initMaxStepLength'......................................................1

'runtimeMaxNumIterations'................................................1

'runtimeMinStepLength'................................................0.01

'runtimeMaxStepLength'...................................................1

'mpcDesiredFrequency'...................................................50

'mrtDesiredFrequency'..................................................400

=============================================================================

Rollout Settings:

=============================================================================

'AbsTolODE'..........................................................1e-05

'RelTolODE'..........................................................0.001

'maxNumStepsPerSecond'...............................................10000

'timeStep'............................................................0.01

'integratorType'.....................................................ODE45 (default)

'checkNumericalStability'................................................0

'reconstructInputTrajectory'.............................................1 (default)

'rootFindingAlgorithm'...................................................0 (default)

'maxSingleEventIterations'..............................................10 (default)

'useTrajectorySpreadingController'.......................................0 (default)

=============================================================================

Swing Trajectory Config:

=============================================================================

'liftOffVelocity'......................................................0.2

'touchDownVelocity'...................................................-0.4

'swingHeight'..........................................................0.1

'swingTimeScale'......................................................0.15

=============================================================================

Initial Modes Schedule:
event times: {0.5}
mode sequence: {15, 15}

Default Modes Sequence Template:
Template switching times: {0, 1}
Template mode sequence: {15}

[CppAdInterface] Compiling Shared Library: /tmp/ocs2/dynamics_systemFlowMap/cppad_generated/dynamics_systemFlowMap_libcppadcg_tmp65741332.so
[ INFO] [1629533323.634868827]: rviz version 1.14.8
[ INFO] [1629533323.635052159]: compiled against Qt version 5.12.8
[ INFO] [1629533323.635091688]: compiled against OGRE version 1.9.0 (Ghadamon)
[ INFO] [1629533323.643808641]: Forcing OpenGl version 0.
[legged_robot_dummy-4] process has finished cleanly
log file: .ros/log/ffeea4d0-0256-11ec-ae58-db1db3bbff06/legged_robot_dummy-4*.log
[legged_robot_target-5] process has finished cleanly
log file: .ros/log/ffeea4d0-0256-11ec-ae58-db1db3bbff06/legged_robot_target-5*.log
[legged_robot_gait_command-6] process has finished cleanly
log file: .ros/log/ffeea4d0-0256-11ec-ae58-db1db3bbff06/legged_robot_gait_command-6*.log
[ INFO] [1629533324.329155885]: Stereo is NOT SUPPORTED
[ INFO] [1629533324.329341591]: OpenGL device: GeForce RTX 3070 Laptop GPU/PCIe/SSE2
[ INFO] [1629533324.329368270]: OpenGl version: 4.6 (GLSL 4.6).
[ WARN] [1629533324.510231347]: link 'imu_link' material 'orange' undefined.
[ WARN] [1629533324.510271435]: link 'imu_link' material 'orange' undefined.
[CppAdInterface] Renaming /tmp/ocs2/dynamics_systemFlowMap/cppad_generated/dynamics_systemFlowMap_libcppadcg_tmp65741332.so to /tmp/ocs2/dynamics_systemFlowMap/cppad_generated/dynamics_systemFlowMap_lib.so
WARNING: Loaded at least one default value in matrix: "Q"
WARNING: Loaded at least one default value in matrix: "R"
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/LF_FOOT_position/cppad_generated/LF_FOOT_position_libcppadcg_tmp46441332.so
[CppAdInterface] Renaming /tmp/ocs2/LF_FOOT_position/cppad_generated/LF_FOOT_position_libcppadcg_tmp46441332.so to /tmp/ocs2/LF_FOOT_position/cppad_generated/LF_FOOT_position_lib.so
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/LF_FOOT_velocity/cppad_generated/LF_FOOT_velocity_libcppadcg_tmp47441332.so
[CppAdInterface] Renaming /tmp/ocs2/LF_FOOT_velocity/cppad_generated/LF_FOOT_velocity_libcppadcg_tmp47441332.so to /tmp/ocs2/LF_FOOT_velocity/cppad_generated/LF_FOOT_velocity_lib.so
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/LF_FOOT_orientation/cppad_generated/LF_FOOT_orientation_libcppadcg_tmp13141332.so
[CppAdInterface] Renaming /tmp/ocs2/LF_FOOT_orientation/cppad_generated/LF_FOOT_orientation_libcppadcg_tmp13141332.so to /tmp/ocs2/LF_FOOT_orientation/cppad_generated/LF_FOOT_orientation_lib.so
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/RF_FOOT_position/cppad_generated/RF_FOOT_position_libcppadcg_tmp26941332.so
[CppAdInterface] Renaming /tmp/ocs2/RF_FOOT_position/cppad_generated/RF_FOOT_position_libcppadcg_tmp26941332.so to /tmp/ocs2/RF_FOOT_position/cppad_generated/RF_FOOT_position_lib.so
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/RF_FOOT_velocity/cppad_generated/RF_FOOT_velocity_libcppadcg_tmp71941332.so
[CppAdInterface] Renaming /tmp/ocs2/RF_FOOT_velocity/cppad_generated/RF_FOOT_velocity_libcppadcg_tmp71941332.so to /tmp/ocs2/RF_FOOT_velocity/cppad_generated/RF_FOOT_velocity_lib.so
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/RF_FOOT_orientation/cppad_generated/RF_FOOT_orientation_libcppadcg_tmp58341332.so
[CppAdInterface] Renaming /tmp/ocs2/RF_FOOT_orientation/cppad_generated/RF_FOOT_orientation_libcppadcg_tmp58341332.so to /tmp/ocs2/RF_FOOT_orientation/cppad_generated/RF_FOOT_orientation_lib.so
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/LH_FOOT_position/cppad_generated/LH_FOOT_position_libcppadcg_tmp4541332.so
[CppAdInterface] Renaming /tmp/ocs2/LH_FOOT_position/cppad_generated/LH_FOOT_position_libcppadcg_tmp4541332.so to /tmp/ocs2/LH_FOOT_position/cppad_generated/LH_FOOT_position_lib.so
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/LH_FOOT_velocity/cppad_generated/LH_FOOT_velocity_libcppadcg_tmp28641332.so
[CppAdInterface] Renaming /tmp/ocs2/LH_FOOT_velocity/cppad_generated/LH_FOOT_velocity_libcppadcg_tmp28641332.so to /tmp/ocs2/LH_FOOT_velocity/cppad_generated/LH_FOOT_velocity_lib.so
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/LH_FOOT_orientation/cppad_generated/LH_FOOT_orientation_libcppadcg_tmp59141332.so
[CppAdInterface] Renaming /tmp/ocs2/LH_FOOT_orientation/cppad_generated/LH_FOOT_orientation_libcppadcg_tmp59141332.so to /tmp/ocs2/LH_FOOT_orientation/cppad_generated/LH_FOOT_orientation_lib.so
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/RH_FOOT_position/cppad_generated/RH_FOOT_position_libcppadcg_tmp52641332.so
[CppAdInterface] Renaming /tmp/ocs2/RH_FOOT_position/cppad_generated/RH_FOOT_position_libcppadcg_tmp52641332.so to /tmp/ocs2/RH_FOOT_position/cppad_generated/RH_FOOT_position_lib.so
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/RH_FOOT_velocity/cppad_generated/RH_FOOT_velocity_libcppadcg_tmp13841332.so
[CppAdInterface] Renaming /tmp/ocs2/RH_FOOT_velocity/cppad_generated/RH_FOOT_velocity_libcppadcg_tmp13841332.so to /tmp/ocs2/RH_FOOT_velocity/cppad_generated/RH_FOOT_velocity_lib.so
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/RH_FOOT_orientation/cppad_generated/RH_FOOT_orientation_libcppadcg_tmp51341332.so
[CppAdInterface] Renaming /tmp/ocs2/RH_FOOT_orientation/cppad_generated/RH_FOOT_orientation_libcppadcg_tmp51341332.so to /tmp/ocs2/RH_FOOT_orientation/cppad_generated/RH_FOOT_orientation_lib.so
WARNING: Failed to set threads priority (one possible reason could be that the user and the group permissions are not set properly.)
WARNING: Failed to set threads priority (one possible reason could be that the user and the group permissions are not set properly.)
[ INFO] [1629533400.919630067]: MPC node is setting up ...
[ INFO] [1629533400.927968340]: Publishing SLQ-MPC messages on a separate thread.
[ INFO] [1629533400.928009266]: MPC node is ready.
[ INFO] [1629533400.928021349]: Start spinning now ...

#####################################################
#####################################################
################# MPC is reset. ###################
#####################################################
#####################################################
terminate called after throwing an instance of 'std::runtime_error'
what(): DDP controller does not generate a stable rollout.
[legged_robot_mpc-3] process has died [pid 41332, exit code -6, cmd
ocs2_ws/devel/lib/ocs2_legged_robot/legged_robot_mpc legged_robot mpc ocs2_ws/src/ocs2/ocs2_robotic_examples/ocs2_legged_robot/config/command/targetTrajectories.info legged_robot_description __name:=legged_robot_mpc __log:=/home/usrname/.ros/log/ffeea4d0-0256-11ec-ae58-db1db3bbff06/legged_robot_mpc-3.log].
log file: .ros/log/ffeea4d0-0256-11ec-ae58-db1db3bbff06/legged_robot_mpc-3*.log

Would you plz give us a detail info about how to use full centroidal dynamics(FCD)? or are there some config i havent changed for FCD?

Hi,

I believe the listed dependency in the documentation should be

Boost C++ (v1.71)

instead of

Boost C++ (v1.54)

(or otherwise it leads to errors such as as boost/core/null_deleter.hpp missing)

Setup: Ubunty 20.04, ros noetic

Thanks!
Stefanos

/home/robot/ws_perceptive_mpc/src/ocs2/ocs2_core/include/ocs2_core/constraint/StateConstraint.h:48:29: error: enclosing class of constexpr non-static member function ‘ocs2::ConstraintOrder ocs2::StateConstraint::getOrder() const’ is not a literal type
I use cmake 3.14.7 in ubuntu16.04 kinetic environment

Dear OCS2 team,

I've found out that when the robot is standing, and we give a target distance of 0.5m forwards (as shown in the figure), the base trajectory (red curve) cannot be fully tracked. This won't happen when the robot is walking. So I was wondering if there is a bug for the stance case?

Best

Hi there,

thanks for providing such an amazing library.

During looking through the library, I realized that there is no information about the usage of ocs2 in the physical environment, like Gazebo. So I was wondering if some examples about using ocs2 in Gazebo could be provided?

Looking forward to hearing from you. Many thanks!

Best
Edward

/home/robot/ws_perceptive_mpc/src/ocs2/ocs2_pinocchio/ocs2_pinocchio_interface/src/PinocchioInterface.cpp:42:63: required from here
/usr/include/boost/variant/detail/make_variant_list.hpp:40:46: error: wrong number of template arguments (21, should be at least 0)
typedef typename mpl::list< T... >::type type;

I use cmake3.14.7 in ubuntu16.04 kinetic and boost1.58

Hi,

thanks for the contribution of ocs2.

Recently I was running the legged robot example provided in the ocs2 directory. A green desired body trajectory will be generated when setting the desired position and yaw angle. However, after moving the robot forward and backward several times, this green trajectory started to float upwards, although the robot motion seemed still correct and logical. I was wondering is that a bug? Or perhaps is it an issue with some dependency versions (like eigen3)? I am using Ubuntu 20.04, eigen3 3.3.7, and I can catkin build the whole ocs2 project without any problems.

Here are some pictures and videos:
Pic:

Video:
https://user-images.githubusercontent.com/39060488/140451959-137e472c-dd27-4414-b1a7-a7e289d618c3.mp4

Thank you in advance.

Best

Hi, I was just curious how this quaternion distance was derived? The result is a 3x1 vector which seems to suggest that it is similar to angular velocity, but this may just be some custom measure?

I'm used to seeing quaternion distance (or quaternion error) in terms of omega = log(q_error). Where q_error is the rotation needed to bring q to qRef, and omega is the angular velocity representation of this rotation.

So for body frame rotations:
qRef = q * q_error
q_error = inv(q)*qRef

For world frame rotations:
qRef = q_error * q
q_error = qRef*inv(q)

Are they different? If so, why this definition for quaternion distance?

Thanks!

Dear ocs2 development team,

First and foremost congratulations for the great work and thank you for sharing this with the community.

I am exploring ocs2 for our quadruped bi-manual Centauro robot which is quite challenging due to the dimension of the system (total 39 actuated DoFs). I would like to ask if you could please provide any insights about the efficiency of the framework for larger systems? E.g. how large systems can ocs2 handle for real-time planning based on your experience? Which solver is more promising in this direction? As far as I know from your published work ocs2 has shown to be efficient for a system including Anymal C + 4 DoF arm + manipulated object dynamics (real-time MPC found in https://arxiv.org/pdf/2103.00946.pdf), but this is still quite far from the Centauro case.

I tried to create a simple example for our robot similar to your legged robot example using the continuous DDP and SRBD model. For now I have kept exactly the same constraints with your legged robot example with a different cost (instead of penalizing feet EE cartesian velocities the whole control input vector is penalized). Only the 4 feet EE are considered as contact points. I have tuned some task parameters so as to target motion suitable for the specific robot (in short I have set smaller touchDownVelocity, liftOffVelocity, targetDisplacementVelocity, mpcDesiredFrequency, larger eventTimes and switchingTimes). You can check my full .info files here. As you can see on the video below I am getting the following output:
WARNING: The solution time window might be shorter than the MPC delay!.

Any advice on how to increase the efficiency of the current formulation?

Finally I would like to ask if it is possible to configure the framework for offline trajectory optimization and what is the suggested changes in the code for this?

Thank you in advance!

Hi ocs2 team.

Thanks for open sourcing your repo. This is extremely helpful to the legged community! When reading the ocs2_centroidal_model, there are some parts that I cannot figure out by myself and hopefully can get some help from you guys. It's briefly discussed in #15 but more information will be helpful.

In CentroidalModelPinocchioMapping.cpp, to create a mapping between the ocs2 state and pinocchio state, we need to get all the partial derivatives, but I'm not quite how you determine some of them. If we write down the centroidal dynamics as:

If we write the centroidal dynamics as an implicit function:

The differential is then:

Then if I understand correctly, ocs2 state $x$ and control $u$, pinocchio position $q$ and velocity $v$ are defined as:

To goal is to find the following jacobians:

By following the implicit differential theorem, some of the terms make sense, for example, if we wish to compute the partial $\frac{\partial{\dot{q}}_b}{\partial{q}_b}$, set $\text{d} {q}_b = {I}$, and all other differentials, except $\text{d} {\dot{q}}_b$, to ${0}$, and solve for $\text{d} {\dot{q}}_b$. The resulting expression is the desired analytical partial derivative.

similarly,

However, if we try to derive this one $\frac{\partial {\dot{q}}_j}{\partial {\bar{h}}_G}$:

It is not clear what the analytical expression for $\frac{\partial {\dot{q}}_j}{\partial {\bar{h}}_G}$ is from the above equation. However, it's clear that $\frac{\partial {\dot{q}}_j}{\partial {\bar{h}}_G} \neq {0}$. Similarly, the entire bottom row block of $\frac{\partial {v}}{\partial {x}}$ shouldn't be ${0}$, but left as ${0}$ here

If we try another partial derivative, $\frac{\partial {q}_b}{\partial {q}_j}$:

Or $\frac{\partial {q}_j}{\partial {q}_b}$:

Again, similar to $\frac{\partial {\dot{q}}_j}{\partial {\bar{h}}_G}$, it's not clear what the analytical expression should be for $\frac{\partial {q}_j}{\partial {q}_b}$ given the relation derived in above equation.

Is there anything I was missing in deriving these partial derivatives?

Thanks a lot in advance.

When I'm compiling the package ocs2_legged_robot in ocs2_robotic_examples, the catkin needs package anymal_c_simple_description, but I couldn't find it anywhere

We are trying to interface OCS2 MPC with some HMIs to dynamically set the target / reference for the MPC to track. We are currently playing around the cart pole example (cart pole dynamic is simulated in a third party simulator CoppeliaSim).

Goal:
The objective is that, a separate ros node will publish a "ocs2_msgs::mpc_target_trajectories" message, such that if the user push a joystick, that cart-pole system will try to follow the user's command and move along the x direction while keeping the pole balanced.

Progress:
We take the ballbot example as our reference while enabling the "targetTrajectoriesSubscriber_". After adding the following lines in the MpcNode.cpp:

  // ROS ReferenceManager
  std::shared_ptr<ocs2::RosReferenceManager> rosReferenceManagerPtr(
      new ocs2::RosReferenceManager(robotName, PurePlanarInterface.getReferenceManagerPtr()));
  rosReferenceManagerPtr->subscribe(nodeHandle);

  // MPC
  ocs2::MPC_DDP mpc(PurePlanarInterface.mpcSettings(), PurePlanarInterface.ddpSettings(), PurePlanarInterface.getRollout(),
                    PurePlanarInterface.getOptimalControlProblem(), PurePlanarInterface.getInitializer());
  mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr);

The MPC node seems to subscribe to the "XX_mpc_target" topic:

And the cart-pole system does try to follow the target state slightly / slowly...
https://www.youtube.com/watch?v=FfF7X0wFsmE
In this video, you will see two cart-pole systems. The green and slightly transparent one indicates the reference states from the "XX_mpc_policy" topic. While moving the target does effect the position of the cart, the reference states indicate the cart always want to go back to the origin.

Questions:
Comparing the task.info files between cart-pole and ballbot, we realize there are more to it:

1. What's the difference between Q and Q_f? (the Q_f for ballbot are all zeros, while the Q for cart-pole are all zeros)
2. We suspect the "x_final" setting in the cart-pole's task.info is what causes the cart to always want to go back to the origin. (we notice that this setting does not exist in the ballbot's task.info) If so, what should we do about it? (Simply excluding it only cause the cart-pole to be unstable) And why is it not included in the ballbot example?
3. How fast can we send the "XX_mpc_target" topic? In the ballbot example, this topic is sent only when there is a new command line entry occurs. If we update / send this topic too fast, will it cause the MPC solver to be unstable? Or it is fine as long as the delta pose is reasonably bounded?

Thank you and looking forward to hearing from you! : )

Describe the bug
what(): DDP controller does not generate a stable rollout.

To Reproduce
Steps to reproduce the behavior:
I basically modified the mobile manipulator example by changing the UR robotics arm to a Kinova robotics arm, and I have ignored the constraints from the self collision

Expected behavior
The mobile manipulator with kinova arm should converge its end-effector to the interactive marker.

Screenshots
If applicable, add screenshots to help explain your problem.

I would like to understand from the algorithmic aspect why this error occurs, thanks for any suggestions or guidance!

Hi,

Is CppAdInterface thread-safe? I used CppAdInterface for constraint and dynamics calculation but encountered some multithreading memory problems. On memory allocation/de-allocation of Eigen matrix or std::vector throughout the pipeline (from getting constraint Jacobian to the destruction of ocs2::ModelData), the debugger reports memory problem randomly including double free and invalid pointer. However with single threaded SLQ, everything is fine.

I didn't use the provided MPC-ROS interface; instead, I directly call run(...,...) of GaussNewtonDDP_MPC. The official example of anymal runs well on my computer so I think I may have ignored some important steps.

Dear ocs2 developers,

I am looking into using ocs2 for implementing an MPC controller for a ship. Impressive library! I have got the library running and was wondering if you could help me with a small query:

In the library documentation, the optimal control problem is given with an inequality constraint h(x,u,t) >= 0. However, I can only see a way to set an inequality Lagrangian (see the attached screenshot). For our application, we would need hard inequality constraints on the state and input, and I was wondering if there is a way to set this h(x,u,t) function instead of using the inequality lagrangian?

Our model is as of now a simple forced harmonic oscillator, and the application is very similar to the double integrator example. Furthermore, due to hardware reasons, I cannot use ROS and am calling the optimization algorithms directly through the advanceMpc method.

Dear authors,

First of all, thank you for making this great toolbox open source! Does this toolbox support offline optimization? Is it as simple as solving the optimal control problem once instead of repeatedly online? My plan is to generate an energy optimal gait offline and to track it with a custom controller.

And second (although I do not know if this is the correct place to ask), I would like to model the dynamic effects of elastic actuators, i.e. series elastic actuators, variable stiffness actuators, etc., but URDF nor Pinocchio support this natively. I know that ANYmal has SEAs, but its dynamics do not seem to be considered. Do you know if it is possible to change the auto-generated code by Pinocchio to incorporate the elastic joints?

Hello, thank you for sharing this amazing repo.

While going through the osc2_centroidal_model example, I found some inconsistency. In the getOcs2Jaconian() functions of CentroidalModelPinocchioMappingTpl, this line, Ab inv is pre-multiplied by robot mass. But when updating the right cols of 'floatingBaseVelocitiesDerivativeState', for both FCD and SRBD cases here and here, robot mass is not being pre-multiplied.

Is this a bug, since the arguments of this function Jq and Jv are both computed using normalized momentum?

Is there any available (in ROS Noetic) obstacle avoidance implementation that I can run as an example? I know that leggedrobotics/perceptive_mpc has that feature but it is not compiling in ROS Noetic and it seems like it is outdated after the recent updates on OCS2. I would appreciate if you can guide me on the implementation or point to any branch or repository that has the feature within the OCS2 framework.

I am using ros noetic on ubuntu 20.04.
After setting up the environment and installing all dependencies, I am running catkin build ocs2 and getting the error:

I would appreciate any help.

Hi all,

I find that OCS2 use global or world frame as generalized velocity's reference, like:

But, In stack-of-tasks/pinocchio#1357, we need to use local or body frame as generalized velocity's reference. So which is correct? I'd appreciate it if you tell me this point.

Hi,

I think JumpMap with input, i,e,. the one documented in SystemDynamicsBase

$ x^+ = G \delta x + H \delta u \f$

is not yet implemented since the interface of systemDynamics supports only

  virtual VectorFunctionLinearApproximation jumpMapLinearApproximation(scalar_t t, const vector_t& x, const PreComputation& preComp);

Hi, thanks for the great work. Do you have any plans to port OCS2 to ROS2?

All the "goal" are spelled as "gaol".

Version: 11.0
Code:

scalar_t MultidimensionalPenalty::getValue(scalar_t t, const vector_t& h, const vector_t* l) const {
  const auto numConstraints = h.rows();
  assert(penaltyPtrArray_.size() == 1 || penaltyPtrArray_.size() == numConstraints);

  scalar_t penalty = 0;
  for (size_t i = 0; i < numConstraints; i++) {
    const auto& penaltyTerm = (penaltyPtrArray_.size() == 1) ? penaltyPtrArray_[0] : penaltyPtrArray_[i];
    penalty += penaltyTerm->getValue(t, getMultiplier(l, i), h(i));
  }

  return penalty;
}

The getMultiplier triggered segmentation fault because l is an uninitialized empty vector. The problem occurs the first time the preJump constraint is active. The horizon is 1s, at 1s there is a mode switch, and the constraint is expected active at the switch.

scalar_t getMultiplier(const vector_t* l, size_t ind) {
  return (l == nullptr) ? 0.0 : (*l)(ind);
}

I'm curious on ocs2 msg details here

;; Normalized Centroidal Momentum: [linear, angular] ;;
   (0,0)  0.0     ; vcom_x
   (1,0)  0.0     ; vcom_y
   (2,0)  0.0     ; vcom_z
   (3,0)  0.0     ; L_x / robotMass
   (4,0)  0.0     ; L_y / robotMass
   (5,0)  0.0     ; L_z / robotMass

the comment on top of the message says Normalized Centroidal Momentum: [linear, angular] where as comments on the following lines state first three components are linear velocities and next three seem to be rotational velocities, rather than momentum?
Another question is that rotational components are stated as L_x / robotMass which I'm assuming L_x represents rotational moment? but why is it divided by robotMass rather than inertia around x-axis?

Dear legged robotics group,

thanks for providing the OCS2.

Recently, we are going to use OCS2 to generate an online walking trajectory for Anymal C and simulate it in the RaiSim. We have developed our WBC to track the reference trajectory. At the very beginning, we employed the MPC solver of OCS2 only as a pure trajectory generator (meaning there was no robot's state feedback obtained from RaiSim into the MPC). But then, we realized that there was a drift in the z-direction, as mentioned in #24. So we tried to find the solution.

During figuring out a solution, we found that #23 mentioned that we could add some feedback terms to the MPC. Moreover, paper "A Unified MPC Framework for Whole-Body Dynamic Locomotion and Manipulation" also mentioned that we could obtain the state information from the robot and insert it into the SLQ-MPC solver, as shown in figure 3. Therefore, we tried to obtain the state from RaiSim and insert it into the MPC solver.

We've found that there is a user-defined modification function interface in file "ocs2_ros_interface/src/mrt/MRT_ROS_Dummy_Loop.cpp". In my opinion, the "observation_" term should be the real robot state that is the input for the MPC. We replaced the "observation_" with the state and input vector that gathered from the anymal in RaiSim. The RaiSim and WBC were ruining at 1kHz, mrtDesiredFrequency was 1khz, and mpcDesiredFrequency was -1 Hz (which was 250Hz in my pc). We tried replacing the "observation_" with measured data in 1kHz. It turned out that even though I set the target position, the robot was standing still on the ground without any motion at first, then it would drift, as shown below:

I thought maybe we were updating the "observation_" too frequently, so I set the updating frequency to 1Hz. But in this way, the robot in rviz (on the left hand side) will “flash” from one place to the other, which will again make the robot in RaiSim unstable, as shown here:

Of course, we have tried different mrtDesiredFrequency, mpcDesiredFrequency and "observation_" updating frequency, but the robot was still unstable. We started to consider that maybe the way we used the measured data for the MPC solver was not correct, because by doing our way, the desired state and input vector seems not continuous, which will, in turn, cause the unstable tracking performance. And also, if we update the "observation_" too frequently, then the generated reference trajectory is too close to the current robot state after each time's MPC update, which won't lead the robot to move. So I was wondering:

1. How to use the robot's measured data for the MPC solver properly?
2. How should we solve the drift problem in z-direction if we want to use the MPC as a pure trajectory planner (without state feedback)?

Many thanks

Best
Shengzhi

In the legged robot example the default tracking cost penalizes deviation of the state and input from desired ones. The latter are continuously updated by the legged_robot_mpc_target topic. However in case we use the legged_robot_mpc_target topic for a different type of target e.g. for an end effector pose (as is done with a soft constraint in the mobile manipulator example) the solver explodes since the default tracking cost is updated with TargetTrajectories that have different size (which in the case of end effector pose is 7). As a result one cannot combine the default tracking cost with a soft constraint for end effector tracking. In other words using a single topic for TargetTrajectories that are used by different cost terms creates the issue. (In the mobile manipulator example this is not the case since the default tracking cost uses only the TargetTrajectories.inputTrajectory while the end effector soft constraint uses the TargetTrajectories.stateTrajectory thus they are not coupled.)

As a solution I have thought of the following options:

1. Creating a separate ros topic for publishing the TargetTrajectories related with the end effector, modify the RosReferenceManager to subscribe to an extra topic and create an extra buffer in the ReferenceManager for the TargetTrajectories of the end effector. This will require modification of code in "core" packages of the library like ocs2_oc so I don't know if this is the best way to go.
2. A second option would be to again create a separate ros topic for the TargetTrajectories of the end effector but create a ros subscriber within the definition of the end effector constraint which will be included in the ocs2_legged_robot. However this has the disadvantage of engaging ros in the ocs2_legged_robot package that is supposed to be free of.

What is your suggestion for solving this?
Thank you!

when I tried to clone the hpp-fcl pkg, the error happens like that
fatal: 无法克隆 'git://github.com/jrl-umi3218/jrl-cmakemodules.git' 到子模组路径 '/home/xwm/桌面/workplace/hpp-fcl/cmake'
克隆 'cmake' 失败。按计划重试
正克隆到 '/home/xwm/桌面/workplace/hpp-fcl/third-parties/qhull'...
fatal: 无法连接到 github.com：
github.com[0: 192.30.253.113]: errno=连接超时
github.com[1: 192.30.253.113]: errno=连接超时

While the provided dummy tests are very useful for studying mpc policy behaviors and weight tuning, they aren't rigorous validations since the generated states are based on the same dynamic models. Currently, we are trying to interface OCS2 with CoppeliaSim which so happens to simulate our robot behaviors well. The naive thought was to simply replace dummy_test with CoppeliaSim's ROS interface (this node will publish /mpc_observation and subscribe to /mpc_policy). We soon realized that by excluding dummy_test node, we also disable MRT ROS interface, which supposes to run along side with MPC (handling reset and timing control). Therefore, we wonder:

• Is MRT interface absolutely necessary or it can be replaced if a third party robot simulator handles the time control? (MPC computes traj based on simulation time)
• How do we enable state-based and feedback policy setting for the MPC solver and MRT? What are the pros and cons compared to time-based?
• Is it possible to provide a DummyXXbotNode.cpp or a MRT_ROS_Interface.cpp temple that allows people to take state observations from other ROS node (i.e., CoppeliaSim) while still having MRT to regulate the timing?

(Sorry, just started using OCS2, I hope the questions make sense. )

Hi,

I am trying to add a soft constraint (EndEffectorConstraint similar to mobile manipulator example) for an arm EE tracking of a quadruped manipulator. This is done by adding an additional cost term on top of the default costs on state and input which are defined in the ocs2_legged_robot example. For tracking an EE pose a new buffer is created at the ReferenceManager and a new ros topic at the RosReferenceManager for communicating TargetTrajectories of the EE pose (although not sure if this is the best way to go, see issue 49).

I am not achieving good tracking and it does not seem to be a tuning issue. I have noticed the below unexpected behavior:

Within the EndEffectorConstraint::getValue() I print the position of the EE to be tracked through endEffectorKinematicsPtr_->getPosition(state).front() and I am comparing it with the published frames in the /tf topic (by simply rosrun tf tf_echo odom ee_frame). When the frame to be tracked belongs to the lower body (frames up to torso_2 link for centauro) it seems to work well and the position is consistent. In contrast, for any of the frames of the arm links the endEffectorKinematicsPtr_ gives position that is way different from the tf.

Note that:

• endEffectorKinematicsPtr_ points to a PinocchioEndEffectorKinematicsCppAd object constructed similarly with the feet EE kinematics in your ocs2_legged_robot ddp example. In particular with the following piece of code:

// define EE kinematics object
std::unique_ptr<EndEffectorKinematics<scalar_t>> eeKinematicsPtr;
const auto infoCppAd = centroidalModelInfo_.toCppAd();
const CentroidalModelPinocchioMappingCppAd pinocchioMappingCppAd(infoCppAd);
auto velocityUpdateCallback = [&infoCppAd](const ad_vector_t& state, PinocchioInterfaceCppAd& pinocchioInterfaceAd) {
  const ad_vector_t q = centroidal_model::getGeneralizedCoordinates(state, infoCppAd);
  updateCentroidalDynamics(pinocchioInterfaceAd, infoCppAd, q);
};
eeKinematicsPtr.reset(new PinocchioEndEffectorKinematicsCppAd(*pinocchioInterfacePtr_, pinocchioMappingCppAd, {name},
                                                              centroidalModelInfo_.stateDim, centroidalModelInfo_.inputDim,
                                                              velocityUpdateCallback, name, modelSettings_.modelFolderCppAd,
                                                              modelSettings_.recompileLibrariesCppAd, modelSettings_.verboseCppAd));

• Since EndEffectorConstraint::getValue() is called multiple times along the horizon of the optimization I get the EE position at those times. In my comparisons with the /tf I actually use the first EE position which corresponds to the time closest to the current one. (In any case the EE position does not change much along the horizon since I am comparing after the robot has entered the "steady state response", i.e. it does not really move.)

• The state vector passed as argument to the endEffectorKinematicsPtr_->getPosition(state) is consistent with the state observation (from /legged_robot_mpc_observation topic).

I was wondering if you have any clues about what can be causing this strange behavior or how to debug this?

Thank you

Describe the bug
-Build error due to no matching function for call to from robot_state_publisher::RobotStatePublisher::publishFixedTransforms(bool)
-this error affects ocs2_ballbot_ros, ocs2_legged_robot and ocs2_mobile_manipulator_ros

• ocs2 and ocs2_robotic_examples are abandoned due to the failing of ocs2_ballbot_ros

To Reproduce
Steps to reproduce the behavior:
following the installation instructions form OCS2 documentation and also installed all the optional dependencies (pinocchio, raisim)
I needed to add "leggedrobotics/anymal_c_simple_description" to src

Expected behavior
successful building of ocs2

Screenshots

Desktop (please complete the following information):

• OS: Ubuntu 18.04.6 LTS
• Browser: Mozilla Firefox 95.0

Thanks for the nice library.
However, when I catkin build, the message ".... src/ocs2/ocs2_robotic_examples/ocs2_robotic_tools does not exist." is shown.
Please move the folder "src/ocs2/ocs2_robotic_tools" to src/ocs2/ocs2_robotic_examples/ocs2_robotic_tools.

Is the implementation of PISOC (Path integral stochastic optimal control) algorithm, presented in this paper, available in OCS2?

• If yes, is there any ready-to-launch example that uses PISOC?
• If there is no example but the implementation is working, could you provide some guidance how can the config file should be set up?

Is your feature request related to a problem? Please describe.
I would like to use hard inequality constraints with the SQP solver (based on HPIPM), rather than just soft constraints (via penalty functions). I would expect hard inequality constraints to be supported by SQP, but after trying to add them, and then digging a bit in the ocs2_sqp package code, it doesn't appear that they are currently supported.

Describe the solution you'd like
Assuming I'm correct that hard inequality constraints are not currently implemented for the SQP solver (happy to be corrected here!), then my preferred solution is for support for them to be added. I would potentially be interested in working on this myself if others see a need for it, too.

Describe alternatives you've considered
Of course, one could just use soft constraints via penalty functions like for DDP-based approaches, but having the alternative of hard inequality constraints is a nice point of comparison for controller behaviour.

Hi,
When I configured my robot using ocs2_legged_robot as an example, the following error occurred:

How should I define my input to ensure that the MPC runs successfully?

Hi, this is an amazing project. I want to deploy this project to our robot, and need to tune the matrix Q,R when mpc is running. However, I don't know how to solve this problem. Could you give me some advices? Thanks!

Hello,

when running the legged_robot_raisim example the pose of the robot immediately changes, the center of mass moves slightly in the x direction and contact forces of the front legs change the orientation. The robot is in some kind of an unstable state and the optimal forces oscillate slightly. After a while, even without any gait command, the right hind leg starts to float in the air and the computation crushes.

Is this a bug or is it due to the use of raisim simulator on a simplified model?

This is the pose of the robot after 2 seconds of the simulation. No gait commanded, no trajectory commanded.

Hi all,

I am using OCS2 to control an OMAV+Delta arm with a velocity driven kinematic model.

I am trying to pass a mpc_target_trajectories message to the mpc in order to follow a reference trajectory where each point represents a desired end-effector pose and twist at a desired time instance. I successfully defined the desired pose by setting the value of the stateTrajectory to the desired position and quaternion, without having to care about the value of the robot state vector. The size of the stateTrajectory value (7) is of course different from the size of the robot state vector (15).
At the moment of writing, I have a problem with the definition of the desired twist. Following the fact that I am controlling the robot with velocity inputs and the way I defined the stateTrajectoy, I tried to define the inputTrajectory as the desired linear and angular velocity of the end-effector. I realized that this approach always created the following error:

It works fine if I define the inputTrajectory value as a vector of input size full of zeros but the results are not satisfying as it always stops at each point. As I have a redundant system, I have no idea what the desired input trajectories should be as they should be the result of the mpc optimization.

How am I able to define desired end-effector twists in the mpc_target_trajectories message?

Thank you in advance for your help!

alme96

Hi,

On Iteration 0 of SLQ solver, I get an exception from the integrator (ODE45_OCS2):

Integration terminated since the maximum number of function calls is reached. State at termination time -0.928457

I initialized the trajectory as

    ocs2::SystemObservation initObservation;
    initObservation.state = interface.initialState;
    initObservation.input = ocs2::vector_t::Zero(robo::INPUT_DIM);
    initObservation.mode = 0;
    // Initial command
    ocs2::TargetTrajectories initTargetTrajectories(
        {0.0}, {initObservation.state}, {initObservation.input});

    // run dummy
    leggedRobotDummySimulator.run(initObservation, initTargetTrajectories);

and the mpc node as

    robo::RobotInterface interface(taskFilePath, targetTrajectoryFilePath);
    /*ros*/
    ros::init(argc, argv, interface.model.name);
    ros::NodeHandle nodeHandle;
    std::shared_ptr<ocs2::RosReferenceManager> rosReferenceManagerPtr(
        new ocs2::RosReferenceManager(interface.model.name, interface.refManager));
    rosReferenceManagerPtr->subscribe(nodeHandle);
    /*mpc*/
    ocs2::MPC_DDP mpc(interface.mpcSetting, interface.ddpSetting,
                      interface.rollout, interface.ocp, interface.initializer);
    mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr);
    ocs2::MPC_ROS_Interface mpcNode(mpc, interface.model.name);
    mpcNode.launchNodes(nodeHandle);

The starting time and initial state are defined in the task.info file. I think it is trying to run iteration on the initial trajectory but have no idea about why it comes to this exception. I wrote the code for dynamics built from pinocchio and CppAD instead of using the provided pinocchio interfaces.
Update
After debugging for a while I found the exception is raised when SLQ integrateRiccatiEquationNominalTime calls integrateTimes.
Update
I tested the fullcentroidaldynamics version of the _leg

ged robot_ example, and soon it becomes unstable.
https://user-images.githubusercontent.com/56295370/152655845-19a45f25-f174-4710-b10a-bb3f68637009.mp4

Describe the bug
An error was reported while running roslaunch ocs2_legged_robot legged_robot.launch

Expected behavior
A clear and concise description of what you expected to happen.

Screenshots

Desktop (please complete the following information):

• OS: ubuntu
• Version: 20.04

OCS2 assumes optimization problems with inequality constitution of type h(x, u) >= 0, and at each iteration of MPC, OCS2 uses the second-order approximation of this inequality constraint.
To find a solution for each sub-problem, the feasible set of the sub-problem should be a convex set. This is the case if the hessian of h(x, u) is n.s.d.
As a simple example for a system with one state and one input: x^2 + u^2 – 1 > 0 is not a convex set but -x^2 - u^2 + 1 > 0 is a convex set.

Originally posted by @farbod-farshidian in #18 (comment)

After I upgraded the packages on my system (Ubuntu 20.04, ROS Noetic) using sudo apt upgrade, the execution of the planned path visualized in Rviz by launching (I didn't test all, but probably all) robotic examples (such as Mabi-Mobile, Kinova Jaco2, Clearpath Ridgeback with UR-5) becomes extremely slow.

Please note that:

1. Although before I was developing in separate branch; after I had this issue, I created another catkin workspace and installed the main branch and all necessary packages according to the instructions. The issue I report here is based on the latest main branch on ocs2.
2. I also tested to run these examples in another computer without these upgrades (I was syncing two computers daily basis so the only differences between them are those upgrades) and there was no issue on that one.

Unfortunately, I could not narrow down which pkg upgrade(s) caused this, but I will include these packages below, hoping that you have a better insight on this weird issue.

Packages that I upgraded (automatically) today (Sep 30, 2022):

google-chrome-stable=106.0.5249.61-1 firefox=104.0+build3-0ubuntu0.20.04.1 firefox-locale-en=104.0+build3-0ubuntu0.20.04.1 libsdformat9-dev:amd64=9.8.0-1~focal sdformat9-sdf=9.8.0-1~focal libsdformat9:amd64=9.8.0-1~focal ros-noetic-genmsg=0.5.16-1focal.20210423.222705 ros-noetic-genpy=0.6.15-1focal.20210423.222823 ros-noetic-message-runtime=0.4.13-1focal.20210423.222947 ros-noetic-gencpp=0.6.5-1focal.20210423.222749 ros-noetic-geneus=3.0.0-1focal.20210423.222918 ros-noetic-genlisp=0.4.18-1focal.20210423.222735 ros-noetic-gennodejs=2.0.2-1focal.20210423.222929 ros-noetic-message-generation=0.4.1-1focal.20210423.223101 ros-noetic-rosbuild=1.15.8-1focal.20210726.192137 ros-noetic-rosconsole=1.14.3-1focal.20210727.062322 ros-noetic-std-msgs=0.5.13-1focal.20210423.223321 ros-noetic-rosgraph-msgs=1.11.3-1focal.20210423.224118 ros-noetic-roscpp=1.15.14-1focal.20220106.233030 ros-noetic-hardware-interface=0.19.5-1focal.20220106.235702 ros-noetic-controller-interface=0.19.5-1focal.20220106.235928 ros-noetic-actionlib-msgs=1.13.1-1focal.20210423.225113 ros-noetic-geometry-msgs=1.13.1-1focal.20210423.223732 ros-noetic-trajectory-msgs=1.13.1-1focal.20210423.224726 ros-noetic-control-msgs=1.5.2-1focal.20210423.225159 ros-noetic-rospy=1.15.14-1focal.20220106.233657 ros-noetic-pluginlib=1.13.0-1focal.20210727.062753 ros-noetic-rosbag-storage=1.15.14-1focal.20220106.234146 ros-noetic-std-srvs=1.11.3-1focal.20210423.223330 ros-noetic-topic-tools=1.15.14-1focal.20220106.234130 ros-noetic-rosbag=1.15.14-1focal.20220106.234447 ros-noetic-rosmsg=1.15.14-1focal.20220106.234759 ros-noetic-rosservice=1.15.14-1focal.20220106.234935 ros-noetic-dynamic-reconfigure=1.7.3-1focal.20220519.084355 ros-noetic-nav-msgs=1.13.1-1focal.20210423.225233 ros-noetic-rosout=1.15.14-1focal.20220106.233651 ros-noetic-roslaunch=1.15.14-1focal.20220106.233833 ros-noetic-rostest=1.15.14-1focal.20220106.233954 ros-noetic-actionlib=1.13.2-1focal.20220106.235021 ros-noetic-realtime-tools=1.16.1-1focal.20220106.235327 ros-noetic-message-filters=1.15.14-1focal.20220106.234151 ros-noetic-rostopic=1.15.14-1focal.20220106.234801 ros-noetic-rosnode=1.15.14-1focal.20220106.234935 ros-noetic-roswtf=1.15.14-1focal.20220106.235010 ros-noetic-sensor-msgs=1.13.1-1focal.20220107.001034 ros-noetic-tf2-msgs=0.7.5-1focal.20210423.225302 ros-noetic-tf2=0.7.5-1focal.20210423.225547 ros-noetic-tf2-py=0.7.5-1focal.20220107.000244 ros-noetic-tf2-ros=0.7.5-1focal.20220107.000508 ros-noetic-tf=1.13.2-1focal.20220107.001704 ros-noetic-rosconsole-bridge=0.5.4-1focal.20210727.062632 ros-noetic-urdf=1.13.2-1focal.20220106.235206 ros-noetic-diff-drive-controller=0.20.0-1focal.20220519.091305 ros-noetic-ackermann-steering-controller=0.20.0-1focal.20220519.094919 ros-noetic-actionlib-tutorials=0.2.0-1focal.20220106.235301 ros-noetic-diagnostic-msgs=1.13.1-1focal.20210423.223644 ros-noetic-diagnostic-updater=1.11.0-1focal.20220106.234234 ros-noetic-amcl=1.17.2-1focal.20220621.180823 ros-noetic-async-web-server-cpp=1.0.3-1focal.20220107.000630 ros-noetic-audio-common-msgs=0.3.15-1focal.20220829.220605 ros-noetic-laser-geometry=1.6.7-1focal.20220107.002427 ros-noetic-map-msgs=1.14.1-1focal.20220107.004909 ros-noetic-visualization-msgs=1.13.1-1focal.20210423.224023 ros-noetic-voxel-grid=1.17.2-1focal.20220621.180435 ros-noetic-costmap-2d=1.17.2-1focal.20220621.180826 ros-noetic-nav-core=1.17.2-1focal.20220621.181513 ros-noetic-base-local-planner=1.17.2-1focal.20220621.181823 ros-noetic-bond=1.8.6-1focal.20210423.223553 ros-noetic-bondcpp=1.8.6-1focal.20220106.234115 ros-noetic-bondpy=1.8.6-1focal.20220107.000248 ros-noetic-bond-core=1.8.6-1focal.20220107.000920 ros-noetic-cv-bridge=1.16.1-1focal.20220906.152322 ros-noetic-image-geometry=1.16.1-1focal.20220906.152439 ros-noetic-camera-calibration=1.16.0-1focal.20220906.154018 ros-noetic-camera-calibration-parsers=1.12.0-1focal.20220107.001835 ros-noetic-image-transport=1.12.0-1focal.20220107.001857 ros-noetic-camera-info-manager=1.12.0-1focal.20220107.003151 ros-noetic-nodelet=1.10.2-1focal.20220106.234419 ros-noetic-capabilities=0.3.1-1focal.20220107.000455 ros-noetic-carrot-planner=1.17.2-1focal.20220621.182611 ros-noetic-resource-retriever=1.12.7-1focal.20211214.164815 ros-noetic-shape-msgs=1.13.1-1focal.20210423.224003 ros-noetic-geometric-shapes=0.7.3-1focal.20220514.015445 ros-noetic-urdfdom-py=0.4.6-1focal.20220107.000752 ros-noetic-srdfdom=0.6.3-1focal.20220201.004735 ros-noetic-kdl-parser=1.14.2-1focal.20220413.173353 ros-noetic-object-recognition-msgs=0.4.2-1focal.20220107.004954 ros-noetic-octomap-msgs=0.3.5-1focal.20210423.223941 ros-noetic-moveit-msgs=0.11.2-1focal.20220107.005220 ros-noetic-tf2-eigen=0.7.5-1focal.20210423.225948 ros-noetic-tf2-geometry-msgs=0.7.5-1focal.20220107.000842 ros-noetic-moveit-core=1.1.9-1focal.20220815.145341 ros-noetic-chomp-motion-planner=1.1.9-1focal.20220815.151800 ros-noetic-clear-costmap-recovery=1.17.2-1focal.20220621.181825 ros-noetic-stereo-msgs=1.13.1-1focal.20220107.005312 ros-noetic-common-msgs=1.13.1-1focal.20220107.010031 ros-noetic-nodelet-tutorial-math=0.2.0-1focal.20220106.234836 ros-noetic-pluginlib-tutorials=0.2.0-1focal.20220106.235108 ros-noetic-turtlesim=0.10.2-1focal.20220106.234424 ros-noetic-turtle-actionlib=0.2.0-1focal.20220106.235404 ros-noetic-common-tutorials=0.2.0-1focal.20220107.002300 ros-noetic-compressed-depth-image-transport=1.14.0-1focal.20220906.154115 ros-noetic-compressed-image-transport=1.14.0-1focal.20220906.153842 ros-noetic-control-toolbox=1.19.0-1focal.20220519.091120 ros-noetic-controller-manager-msgs=0.19.5-1focal.20220107.000302 ros-noetic-controller-manager=0.19.5-1focal.20220107.000501 ros-noetic-costmap-converter=0.0.13-1focal.20220906.152651 ros-noetic-ddynamic-reconfigure=0.3.2-1focal.20220519.092505 ros-noetic-eigen-conversions=1.13.2-1focal.20210423.223900 ros-noetic-depth-image-proc=1.16.0-1focal.20220906.153241 ros-noetic-turtle-tf=0.2.3-1focal.20220107.003050 ros-noetic-turtle-tf2=0.2.3-1focal.20220107.001151 ros-noetic-geometry-tutorials=0.2.3-1focal.20220107.011515 ros-noetic-joint-state-publisher=1.15.1-1focal.20220218.093002 ros-noetic-python-qt-binding=0.4.4-1focal.20210726.192424 ros-noetic-joint-state-publisher-gui=1.15.1-1focal.20220218.093511 ros-noetic-diagnostic-aggregator=1.11.0-1focal.20220107.000450 ros-noetic-diagnostic-analysis=1.11.0-1focal.20220107.000846 ros-noetic-diagnostic-common-diagnostics=1.11.0-1focal.20220107.003636 ros-noetic-self-test=1.11.0-1focal.20220106.234521 ros-noetic-diagnostics=1.11.0-1focal.20220107.003926 ros-noetic-smach-msgs=2.5.0-1focal.20210423.224149 ros-noetic-smach-ros=2.5.0-1focal.20220107.000235 ros-noetic-executive-smach=2.5.0-1focal.20220107.004327 ros-noetic-filters=1.9.2-1focal.20220512.124835 ros-noetic-kdl-conversions=1.13.2-1focal.20210423.223910 ros-noetic-tf-conversions=1.13.2-1focal.20220107.012308 ros-noetic-geometry=1.13.2-1focal.20220107.012748 ros-noetic-tf2-kdl=0.7.5-1focal.20220107.000843 ros-noetic-robot-state-publisher=1.15.2-1focal.20220413.174117 ros-noetic-nodelet-topic-tools=1.10.2-1focal.20220519.092216 ros-noetic-nodelet-core=1.10.2-1focal.20220519.095817 ros-noetic-mk=1.15.8-1focal.20210726.192420 ros-noetic-roslang=1.15.8-1focal.20210726.192150 ros-noetic-ros=1.15.8-1focal.20210727.062118 ros-noetic-roslisp=1.9.24-1focal.20210726.192430 ros-noetic-ros-comm=1.15.14-1focal.20220106.235155 ros-noetic-ros-core=1.5.0-1focal.20220107.010105 ros-noetic-ros-base=1.5.0-1focal.20220519.095901 ros-noetic-xacro=1.14.13-1focal.20220212.160007 ros-noetic-robot=1.5.0-1focal.20220519.102310 ros-noetic-roscpp-tutorials=0.10.2-1focal.20220106.235817 ros-noetic-rospy-tutorials=0.10.2-1focal.20220107.000622 ros-noetic-ros-tutorials=0.10.2-1focal.20220107.001106 ros-noetic-interactive-markers=1.12.0-1focal.20220107.001131 ros-noetic-rviz=1.14.19-1focal.20220815.135636 ros-noetic-urdf-tutorial=0.5.0-1focal.20220815.145010 ros-noetic-interactive-marker-tutorials=0.11.0-1focal.20220107.002345 ros-noetic-librviz-tutorial=0.11.0-1focal.20220815.144408 ros-noetic-rviz-plugin-tutorials=0.11.0-1focal.20220815.144706 ros-noetic-rviz-python-tutorial=0.11.0-1focal.20220815.144829 ros-noetic-visualization-marker-tutorials=0.11.0-1focal.20220106.234737 ros-noetic-visualization-tutorials=0.11.0-1focal.20220815.145402 ros-noetic-qt-gui=0.4.2-1focal.20210726.192753 ros-noetic-rqt-gui=0.5.3-1focal.20220328.224533 ros-noetic-rqt-gui-py=0.5.3-1focal.20220328.225040 ros-noetic-rqt-logger-level=0.4.11-1focal.20220328.225328 ros-noetic-rqt-py-common=0.5.3-1focal.20220328.224538 ros-noetic-rqt-console=0.4.11-1focal.20220328.225357 ros-noetic-rqt-msg=0.4.10-1focal.20220328.225436 ros-noetic-rqt-action=0.4.9-1focal.20220328.225504 ros-noetic-rqt-bag=0.5.1-1focal.20220328.225616 ros-noetic-qt-gui-py-common=0.4.2-1focal.20210726.192821 ros-noetic-rqt-plot=0.4.13-2focal.20220328.225111 ros-noetic-rqt-bag-plugins=0.5.1-1focal.20220328.225810 ros-noetic-qt-dotgraph=0.4.2-1focal.20210726.192724 ros-noetic-rqt-graph=0.4.14-1focal.20220328.225326 ros-noetic-rqt-dep=0.4.12-1focal.20220328.225604 ros-noetic-qt-gui-cpp=0.4.2-1focal.20210727.063105 ros-noetic-rqt-gui-cpp=0.5.3-1focal.20220328.224450 ros-noetic-rqt-image-view=0.4.16-1focal.20220906.153817 ros-noetic-rqt-launch=0.4.9-1focal.20220328.225434 ros-noetic-rqt-publisher=0.4.10-1focal.20220328.225218 ros-noetic-rqt-py-console=0.4.10-1focal.20220328.225605 ros-noetic-rqt-reconfigure=0.5.5-1focal.20220519.091134 ros-noetic-rqt-service-caller=0.4.10-1focal.20220328.225220 ros-noetic-rqt-shell=0.4.11-1focal.20220328.225349 ros-noetic-rqt-srv=0.4.9-1focal.20220328.225517 ros-noetic-rqt-top=0.4.10-1focal.20220328.225430 ros-noetic-rqt-topic=0.4.13-1focal.20220328.225437 ros-noetic-rqt-web=0.4.10-1focal.20220328.225602 ros-noetic-rqt-common-plugins=0.4.9-1focal.20220906.160735 ros-noetic-rqt-moveit=0.5.10-1focal.20220328.225504 ros-noetic-rqt-nav-view=0.5.7-1focal.20220328.225152 ros-noetic-rqt-pose-view=0.5.11-1focal.20220328.225119 ros-noetic-rqt-robot-monitor=0.5.14-1focal.20220328.225806 ros-noetic-rqt-robot-dashboard=0.5.8-1focal.20220328.225954 ros-noetic-rqt-robot-steering=0.5.12-1focal.20220328.225602 ros-noetic-rqt-runtime-monitor=0.5.9-1focal.20220328.225559 ros-noetic-rqt-rviz=0.7.0-1focal.20220815.144625 ros-noetic-rqt-tf-tree=0.6.3-1focal.20220331.120200 ros-noetic-rqt-robot-plugins=0.5.8-1focal.20220815.145413 ros-noetic-viz=1.5.0-1focal.20220906.161859 ros-noetic-desktop=1.5.0-1focal.20220906.162158 ros-noetic-polled-camera=1.12.0-1focal.20220107.003238 ros-noetic-image-common=1.12.0-1focal.20220107.012243 ros-noetic-image-proc=1.16.0-1focal.20220906.153713 ros-noetic-image-publisher=1.16.0-1focal.20220906.153738 ros-noetic-image-rotate=1.16.0-1focal.20220906.152658 ros-noetic-image-view=1.16.0-1focal.20220906.153859 ros-noetic-stereo-image-proc=1.16.0-1focal.20220906.154208 ros-noetic-image-pipeline=1.16.0-1focal.20220906.160026 ros-noetic-theora-image-transport=1.14.0-1focal.20220906.153933 ros-noetic-image-transport-plugins=1.14.0-1focal.20220906.160731 ros-noetic-laser-assembler=1.7.8-1focal.20220512.130035 ros-noetic-laser-filters=1.9.0-1focal.20220519.091721 ros-noetic-laser-pipeline=1.6.4-1focal.20220519.095620 ros-noetic-pcl-msgs=0.3.0-1focal.20220107.005305 ros-noetic-pcl-conversions=1.7.4-1focal.20220216.095102 ros-noetic-pcl-ros=1.7.4-1focal.20220520.061642 ros-noetic-perception-pcl=1.7.4-1focal.20220520.064711 ros-noetic-vision-opencv=1.16.1-1focal.20220906.153138 ros-noetic-perception=1.5.0-1focal.20220906.160811 ros-noetic-gazebo-msgs=2.9.2-1focal.20220107.005048 ros-noetic-gazebo-ros=2.9.2-1focal.20220519.091903 ros-noetic-gazebo-plugins=2.9.2-1focal.20220906.152700 ros-noetic-gazebo-ros-pkgs=2.9.2-1focal.20220906.161250 ros-noetic-stage-ros=1.8.0-1focal.20220107.003039 ros-noetic-simulators=1.5.0-1focal.20220906.161851 ros-noetic-joint-limits-interface=0.19.5-1focal.20220106.235926 ros-noetic-transmission-interface=0.19.5-1focal.20220106.235904 ros-noetic-gazebo-ros-control=2.9.2-1focal.20220519.094658 ros-noetic-joint-state-controller=0.20.0-1focal.20220512.124640 ros-noetic-forward-command-controller=0.20.0-1focal.20220512.124630 ros-noetic-position-controllers=0.20.0-1focal.20220512.124837 ros-noetic-urdf-sim-tutorial=0.5.1-1focal.20220815.145156 ros-noetic-desktop-full=1.5.0-1focal.20220906.162231 ros-noetic-dwa-local-planner=1.17.2-1focal.20220621.182243 ros-noetic-effort-controllers=0.20.0-1focal.20220519.094657 ros-noetic-fake-localization=1.17.2-1focal.20220621.180410 ros-noetic-force-torque-sensor-controller=0.20.0-1focal.20220512.124628 ros-noetic-franka-description=0.10.0-1focal.20220908.212816 ros-noetic-uuid-msgs=1.0.6-1focal.20210423.224434 ros-noetic-geographic-msgs=0.5.6-1focal.20210605.000032 ros-noetic-navfn=1.17.2-1focal.20220621.182116 ros-noetic-global-planner=1.17.2-1focal.20220621.182512 ros-noetic-gmapping=1.4.2-1focal.20220107.002506 ros-noetic-graph-msgs=0.1.0-2focal.20210423.223901 ros-noetic-grid-map-cv=1.6.4-1focal.20220906.154107 ros-noetic-grid-map-msgs=1.6.4-1focal.20220106.234334 ros-noetic-grid-map-ros=1.6.4-1focal.20220906.154457 ros-noetic-grid-map-rviz-plugin=1.6.4-1focal.20220906.160950 ros-noetic-gripper-action-controller=0.20.0-1focal.20220519.094627 ros-noetic-hector-map-tools=0.5.2-4focal.20210423.230125 ros-noetic-hector-compressed-map-transport=0.5.2-4focal.20220906.153759 ros-noetic-hector-gazebo-plugins=0.5.4-1focal.20220519.092542 ros-noetic-hector-nav-msgs=0.5.2-4focal.20210423.230130 ros-noetic-hector-geotiff=0.5.2-4focal.20220106.233828 ros-noetic-hector-geotiff-plugins=0.5.2-4focal.20220107.000855 ros-noetic-hector-trajectory-server=0.5.2-4focal.20220107.002646 ros-noetic-hector-geotiff-launch=0.5.2-4focal.20220107.003903 ros-noetic-hector-imu-attitude-to-tf=0.5.2-4focal.20220107.002542 ros-noetic-hector-marker-drawing=0.5.2-4focal.20220106.234436 ros-noetic-hector-map-server=0.5.2-4focal.20220107.002618 ros-noetic-hector-mapping=0.5.2-4focal.20220107.002900 ros-noetic-hector-slam-launch=0.5.2-4focal.20220815.144625 ros-noetic-hector-slam=0.5.2-4focal.20220906.160751 ros-noetic-hls-lfcd-lds-driver=1.1.2-1focal.20220107.001841 ros-noetic-hpp-fcl=2.1.2-1focal.20220912.181902 ros-noetic-imu-filter-madgwick=1.2.5-1focal.20220823.164534 ros-noetic-imu-sensor-controller=0.20.0-1focal.20220512.125128 ros-noetic-tf2-sensor-msgs=0.7.5-1focal.20220107.002056 ros-noetic-imu-transformer=0.3.0-2focal.20220107.002340 ros-noetic-interactive-marker-twist-server=1.2.2-1focal.20220107.002353 ros-noetic-joint-trajectory-controller=0.20.0-1focal.20220519.094643 ros-noetic-joy=1.15.1-1focal.20220107.001731 ros-noetic-teleop-tools-msgs=0.4.0-1focal.20210423.225515 ros-noetic-joy-teleop=0.4.0-1focal.20220107.002043 ros-noetic-jsk-footstep-msgs=4.3.2-1focal.20210423.225200 ros-noetic-jsk-recognition-msgs=1.2.15-1focal.20220107.005530 ros-noetic-laser-ortho-projector=0.3.3-1focal.20220520.063804 ros-noetic-laser-scan-matcher=0.3.3-1focal.20220520.064608 ros-noetic-laser-scan-sparsifier=0.3.3-1focal.20220107.001631 ros-noetic-laser-scan-splitter=0.3.3-1focal.20220107.001646 ros-noetic-libmavconn=1.13.0-1focal.20220810.202815 ros-noetic-lms1xx=0.3.0-2focal.20220107.001900 ros-noetic-map-server=1.17.2-1focal.20220621.180409 ros-noetic-mavros-msgs=1.13.0-1focal.20220113.084554 ros-noetic-mavros=1.13.0-1focal.20220810.203123 ros-noetic-mbf-abstract-core=0.4.0-1focal.20211026.191919 ros-noetic-mbf-utility=0.4.0-1focal.20220107.002405 ros-noetic-mbf-costmap-core=0.4.0-1focal.20220621.181831 ros-noetic-mbf-msgs=0.4.0-1focal.20211026.191913 ros-noetic-microstrain-3dmgx2-imu=1.5.13-1focal.20220107.002715 ros-noetic-move-base-msgs=1.14.1-1focal.20210423.225212 ros-noetic-rotate-recovery=1.17.2-1focal.20220621.182615 ros-noetic-move-base=1.17.2-1focal.20220621.183020 ros-noetic-move-slow-and-clear=1.17.2-1focal.20220621.182127 ros-noetic-moveit-ros-occupancy-map-monitor=1.1.9-1focal.20220815.151800 ros-noetic-moveit-ros-planning=1.1.9-1focal.20220815.152032 ros-noetic-warehouse-ros=0.9.5-1focal.20220510.081837 ros-noetic-moveit-ros-warehouse=1.1.9-1focal.20220815.153512 ros-noetic-moveit-kinematics=1.1.9-1focal.20220815.154002 ros-noetic-moveit-ros-move-group=1.1.9-1focal.20220815.154251 ros-noetic-moveit-ros-manipulation=1.1.9-1focal.20220815.155527 ros-noetic-moveit-ros-planning-interface=1.1.9-1focal.20220912.182033 ros-noetic-moveit-commander=1.1.9-1focal.20220912.182958 ros-noetic-moveit-planners-chomp=1.1.9-1focal.20220912.183113 ros-noetic-moveit-planners-ompl=1.1.9-1focal.20220815.154137 ros-noetic-pilz-industrial-motion-planner=1.1.9-1focal.20220912.183628 ros-noetic-moveit-planners=1.1.9-1focal.20220912.184813 ros-noetic-moveit-fake-controller-manager=1.1.9-1focal.20220815.153741 ros-noetic-moveit-simple-controller-manager=1.1.9-1focal.20220815.151800 ros-noetic-moveit-ros-control-interface=1.1.9-1focal.20220815.152436 ros-noetic-moveit-plugins=1.1.9-1focal.20220815.154010 ros-noetic-moveit-ros-benchmarks=1.1.9-1focal.20220815.154415 ros-noetic-moveit-ros-perception=1.1.9-1focal.20220906.152649 ros-noetic-moveit-ros-robot-interaction=1.1.9-1focal.20220815.153511 ros-noetic-moveit-ros-visualization=1.1.9-1focal.20220912.183000 ros-noetic-moveit-ros=1.1.9-1focal.20220912.184702 ros-noetic-moveit-setup-assistant=1.1.9-1focal.20220912.184642 ros-noetic-moveit=1.1.9-1focal.20220912.185737 ros-noetic-navigation=1.17.2-1focal.20220621.183641 ros-noetic-ncd-parser=0.3.3-1focal.20220107.002746 ros-noetic-nmea-msgs=1.1.0-1focal.20210423.223822 ros-noetic-nmea-comms=1.2.0-3focal.20220106.234629 ros-noetic-nmea-navsat-driver=0.6.1-2focal.20220107.003644 ros-noetic-octomap-ros=0.4.1-1focal.20220514.015020 ros-noetic-pinocchio=2.6.9-2focal.20220912.185923 ros-noetic-pointcloud-to-laserscan=1.4.1-1focal.20220107.002833 ros-noetic-pointgrey-camera-description=0.15.1-1focal.20220413.175145 ros-noetic-polar-scan-matcher=0.3.3-1focal.20220107.003005 ros-noetic-realsense2-camera=2.3.2-1focal.20220906.153355 ros-noetic-realsense2-description=2.3.2-1focal.20220212.161322 ros-noetic-rgbd-launch=2.3.0-1focal.20220906.160418 ros-noetic-robot-localization=2.7.4-1focal.20220729.124845 ros-noetic-robot-pose-ekf=1.15.0-2focal.20220107.003015 ros-noetic-robot-self-filter=0.1.32-1focal.20220520.064643 ros-noetic-robot-upstart=0.4.2-1focal.20220217.090727 ros-noetic-velocity-controllers=0.20.0-1focal.20220519.094749 ros-noetic-ros-controllers=0.20.0-1focal.20220519.095236 ros-noetic-ros-numpy=0.0.5-2focal.20220107.003648 ros-noetic-rosbridge-library=0.11.14-1focal.20220615.161520 ros-noetic-rosapi=0.11.14-1focal.20220615.161853 ros-noetic-rosauth=1.0.1-1focal.20220106.234716 ros-noetic-rosbridge-msgs=0.11.14-1focal.20220615.161535 ros-noetic-rosbridge-server=0.11.14-1focal.20220615.161932 ros-noetic-rosdoc-lite=0.2.10-1focal.20210423.222829 ros-noetic-rosparam-shortcuts=0.4.0-1focal.20220106.234044 ros-noetic-rospy-message-converter=0.5.9-1focal.20220912.175101 ros-noetic-rosserial-msgs=0.9.2-1focal.20210423.223234 ros-noetic-rosserial-python=0.9.2-1focal.20220107.000351 ros-noetic-variant-msgs=0.1.6-1focal.20210423.224434 ros-noetic-variant-topic-tools=0.1.6-1focal.20220106.234434 ros-noetic-rqt-multiplot=0.0.12-1focal.20220328.224855 ros-noetic-rviz-imu-plugin=1.2.5-1focal.20220823.164227 ros-noetic-rviz-visual-tools=3.9.1-1focal.20220815.145126 ros-noetic-scan-to-cloud-converter=0.3.3-1focal.20220520.064730 ros-noetic-scan-tools=0.3.3-1focal.20220520.065501 ros-noetic-sick-tim=0.0.17-1focal.20220519.091711 ros-noetic-smach-viewer=4.0.1-1focal.20220906.152653 ros-noetic-sound-play=0.3.15-1focal.20220829.221132 ros-noetic-spacenav-node=1.15.1-1focal.20220107.001953 ros-noetic-teb-local-planner=0.9.1-1focal.20220906.160038 ros-noetic-teleop-twist-joy=0.1.3-1focal.20220107.002011 ros-noetic-teleop-twist-keyboard=1.0.0-1focal.20220107.000515 ros-noetic-tf2-tools=0.7.5-1focal.20220107.004507 ros-noetic-tf2-web-republisher=0.3.2-3focal.20220107.002444 ros-noetic-turtlebot3-msgs=1.0.1-1focal.20210423.224157 ros-noetic-twist-mux-msgs=2.1.0-1focal.20220107.004036 ros-noetic-twist-mux=3.1.2-1focal.20220819.064426 ros-noetic-velodyne-description=1.0.12-2focal.20220212.161113 ros-noetic-velodyne-gazebo-plugins=1.0.12-2focal.20220519.100106 ros-noetic-web-video-server=0.2.2-1focal.20220906.154245

Hi,

We are trying to integrate bipedal robot Cassie into Ocs2 based on the legged robot example of ANYmal. We have some issues with Cassie being able to stand with convergence and the solver gives the error below:

Our setup, the center of mass and the locations of four support/contact points of the feet with nominal contact forces are shown below:

The center of mass is within the support polygon while the error is given. We tried both single body centroidal dynamics and full body centroidal dynamics. We tried to tune the cost coefficients and removed some constraint conditions to relax the optimization. None of them worked.

We have been stuck here for a few weeks. Any suggestions to fix this issue from the algorithmic point of view? Thank you very much!

Describe the bug
I am not sure if this is the best place to ask, but I am running out of ideas. I am trying to include the ocs2_ros_interfaces package as well as pcl_ros, and they seem to really dislike each other. I created one node that depends on pcl_ros, and another that depends on ocs2_ros_interfaces. I can build each of them separately but when they are trying to build together, I get a lot of errors. The errors specifically relate to pcl in the code terminal, but I figured I might as well ask here as well in case anyone else has gotten pcl_ros to build with ocs2_ros_interfaces.

To Reproduce
Steps to reproduce the behavior:
Add pcl_ros to the package.xml and CMakeLists.txt, create a node that has #include <pcl/point_types.h> (or any pcl_ros header file). Try to catkin build.

Expected behavior
A clear and concise description of what you expected to happen.

Screenshots
If applicable, add screenshots to help explain your problem.

Desktop (please complete the following information):
Ros Noetic on Ubuntu 20.04

Additional context
Add any other context about the problem here.

Hi, thank you so much for your great work about os2. I want to add some stair model to the rviz. So I can research how ANYmal climb stairs then. But I am new to ros, I don't know how to add models to the rviz. Can you give me some help?

I am currently trying to compile ocs2 with pinocchio. The ocs2_pinocchio_interface has been successfully compiled.

Finished  <<< ocs2_pinocchio_interface   

But when compiling other pkgs such as ocs2_self_collision, it reports:

CMake Error at /home/harper/Apps/ocs2_ws/devel/share/ocs2_pinocchio_interface/cmake/ocs2_pinocchio_interfaceConfig.cmake:173 (message):
  Project 'ocs2_self_collision' tried to find library 'pinocchio'.  The
  library is neither a target nor built/installed properly.  Did you compile
  project 'ocs2_pinocchio_interface'? Did you find_package() it before the
  subdirectory containing its code is included?

By checking the cmake cache of ocs2_pinocchio_interfaces, I can see the pinocchio lib file is found. What can I do to continue the compilation?