How to properly enable and use "targetTrajectoriesSubscriber_"? about ocs2 HOT 4 CLOSED

Updates:

For debug purposes, we hook up the target generating ros node with the ballbot dummy test. And it works great:

https://www.youtube.com/watch?v=fqYUb_yM0xc

What we learned from this test are:

1. The publisher end of "XX_mpc_target" seems ok, as it can control a different robot with slight modifications.
2. The publishing rate of "XX_mpc_target" is not that critical. We are sending / updating it at a rate of 20Hz while the MPC/MRT is running at 100Hz. The performance seems stable enough.

Yet still, that implies the receiving end of "XX_mpc_target" in the cart-pole example is not properly configured.
Can you help us understand the differences between the cart-pole and ballbot setup?

from ocs2.

Q will be the state cost matrix for the intermediate states. Q_f would be the cost matrix for the final state:
problem_.costPtr->add("cost", std::unique_ptr<StateInputCost>(new QuadraticStateInputCost(Q, R))); problem_.finalCostPtr->add("finalCost", std::unique_ptr<StateCost>(new QuadraticStateCost(Qf)));
For sure setting the Q to nonzero values makes sense if you want to have better tracking. In general you will want to set both Q and Q_f. We have not done that for all examples, just for simplicity.

The loaded final state will not play a role as soon as you send a new target trajectory. The initial reference is set here:

Your approach seems fine to me. Make sure you are sending a reference state of the correct size and convention. x = [theta, position, theta_dot, linearVelocity] and also send a reference for the input u = [force]. And if you are sending a trajectory (multiple setpoints) as a reference, you want to look at the timestamps along the trajectory. The MPC cost will linearly interpolate the reference you send. The intermediate and final cost will both use the same reference trajectory.

You can update the references as fast as you like. The code takes care that updating the referencing happens only when a new MPC problem starts and it will just take the latest available. The MPC will always use the previous solution as a warm-start. So in that sense it is beneficial to have the reference updated continuously instead of having it make a big jump. For the cartpole all of that should not be so important.

from ocs2.

@rubengrandia Thank you for your in-depth reply! Really appreciate it!
And sorry that we couldn't get back to you sooner!

It turns out we have a unit convention issue in the target states we sent ( rad vs m ). The pipeline we showed earlier does work:
https://www.youtube.com/watch?v=YXDp7ltvarI

We are now moving towards sending multiple setpoints and maybe trying velocity control instead. And so far, it seems like the cart-pole, ballot, and quadrotor examples are all doing position control (tracking a target position in task space).

• In my naive understanding, the velocity terms in the terminal target state should be always zeros (?) So if we are interested in controlling the speed, we should send our speed reference through intermediate states (multiple setpoints)?
• Or shall we just integrate our speed reference to position reference and do the regular position tracking instead? Which formulation is more elegant in your opinion?
• We also don't have much clue on how to send multiple setpoints as the mpc target. Could you point us in a direction where we can start?

Thank you as always! : )

from ocs2.

The velocity of the terminal target state does not need to be zero, you can set it as you want.

As a first implementation you can set your position costs to zero and set a non-zero velocity as a reference. As you already mention, integrating the commands to maintain a position reference is also a valid idea. I would just experiment a bit and see what works best for your use case.

Multiple a time varying reference with multiple setpoints can be simply commanded by adding more time, states, and inputs to the targetTrajectory. In the example here you see that the trajectory consists of 2 points, but you can just add more points to those vectors if you want.

from ocs2.