issue defining desired twist trajectorie in a mpc_target_trajectories message

[alme96]

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

[Mayankm96]

Hi alme96,

In the current mobile manipulator, we don't make use of the reference input trajectories for tracking and it is pretty much ignored.

The reference trajectories use Eigen dynamic-size vectors, thereby being of any size. You can check this in the implementation of ROS message converter here. It is up to the cost and constraint to interpret how these state and input trajectories are to be interpreted.

For example, if you check the following implementation for end-effector pose-tracking:

https://github.com/leggedrobotics/ocs2/blob/main/ocs2_robotic_examples/ocs2_mobile_manipulator/src/constraint/EndEffectorConstraint.cpp#L107

the "state" trajectory as the end-effector pose (which is of dimension 7 -- position and quaternion). This is different from the state dimension.

For end-effector twist tracking, you'll have to implement something similar. However, instead of a StateConstraint, this will be a StateInputConstraint as it depends on both the current state (joint positions) and the input (desired joint velocities).

You can make use of the EndEffectorKinematics class for getting the corresponding end-effector velocity which you can design into a constraint by using the input trajectories from the reference manager.

https://github.com/leggedrobotics/ocs2/blob/main/ocs2_pinocchio/ocs2_pinocchio_interface/src/PinocchioEndEffectorKinematics.cpp#L100

Since it is a feature that might be interesting for others as well, we would be happy to review your PR with the implementation. Let us know if you have any questions.

[alme96]

Hi Mayankm96,

Thank you for your answer. I tried to implement the linear velocity constraints but without success. I defined the constraints as StateInputConstraint and created an interpolation function that returns the linear and angular velocity as a tuple just like the interpolateEndEffectorPose. Then, I defined the linear approximation with the Pinocchio function getVelocityLinearApproximation, and defined approximation.f, approximation.dfdx and approximation.dfdu.

Here you see what I return in the getValue function:

Here you see what I return in the getLinearApproximation function:

W_v_C1 is the velocity the end-effector frame used by Pinocchio, which in my case is different from the real end-effector of the robot which is not included in the urdf because it is a delta arm which is closed chain and therefore not supported by Pinocchio and OCS2.

The problem I am facing is that after I turn on the mpc_node, it crashes, saying that the R matrix is not positive definite and that the "Intermediate cost second derivative w.r.t. input is not self-adjoint.":

I am having troubles understanding where the problem is because

1. I am defining the input weight matrix R in the task.info file, so I don't know how it can become negative definite.
2. I don't know how it calculates the second derivative of the cost wrt input and what are the parameters that matter

I am defining the constraints as StateInputSoftConstraints and I am using a QuadraticPenalty function. The constraints are added to the OptimalControlProblem through the softConstraintPtr.

Thank you for your help!

[farbod-farshidian]

Answer to question: How could the input weight matrix R (second derivative of the cost wrt input) be a non-positive definite when you defined it as a positive definite in your task file?
Answer: Before passing the cost derivative to the Riccati solver, we add the SoftConstraints penalties and project it to the null space of state-input equality constraints. Therefore, it might be that your penalties, Hessian is not a positive definte.

In your case, try to adjust your QuadraticPenalty coefficients. Moreover, why don't you use hard constraints for this problem? The state-input equality constraints can be handled directly through the projection method.