Use of TargetTrajectories for multiple cost terms r legged robot

[IoannisDadiotis]

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!