Forward dynamics with constraints

[micko-plz]

Hi all,

I'm controlling a mobile manipulator with a two wheel differential drive base (segway like) using the whole body EoM. The control policy is not respecting the constraint I am defining and I'm unsure if its due to lack of understanding of the pinocchio functions I am calling or if its a solver issue so my apologies if this post veers away from purely a pinocchio related dilemma. I am hoping you could confirm whether I am calling the relevant functions correctly via the code brief below.

Specifically:

1. With the torque vector I am giving to the forward dynamics function aba, can I ensure the force being applied along the lateral 'y' direction of the robot's base is zero?
2. Is the second row of the jacobian of the base joint multiplied by the generalised velocities equal to the velocity of the base in its local 'y' frame, ie direction perpendicular to its permitted forward velocity? This jacobian is calculated using computeJointJacobian which is documented as returning the jacobian of the joint expressed in its local frame
3. Are the derivatives of this velocity correct?
4. Should I be using forwardDynamics() with my constraint vector rather than aba()? If so how can derivatives of these dynamics be calculated?

I have simplified the code into what is pasted below, the wheels are neglected and instead assume a force can be applied in the robots local forward/back 'x' direction, and also a torque about its z direction for turning.

%----------------------------------------------------------------------------------------%

// import the urdf with a root joint of planar type
urdf::buildModel(pathToUrdf, JointModelPlanar(), Model_);
Data_ = Data(Model_);
base_idx_ = Model_.getJointId("root_joint");

// the configuration and torque vectors looks as follows:
// q_ = [world_x_position]
//      = [world_y_position]
//      = [heading_angle]
//      = [upper body revolute joint_angles]
//
// tau_ = [force along local_base_x]
//         = [0]
//         = [torque about base_z]
//         = [torque about upper body joint]
//
// Note: q_ is extended and edited for the calculations below such that its 3rd and 4th elements
// are cos(heading) and
// sin(heading) respectively.
//

// with the controlled states being gen coords. "q_", gen vels "qdot_", and the control input being forces/torqes "tau_" the time derivative of
// the gen vels "qddot_" are calculated using the following
qddot = aba(Model_, Data_, q_, qdot_, tau_);

// I also need to provide the derivative of the dynamics with respect to state and input where the rhs terms are calculated using
computeABADerivatives(Model_, Data_, q_, qdot_, tau_, aba_partial_dq_, aba_partial_dv_, aba_partial_dtau_);
dqddot_dq = aba_partial_dq_;
dqddot_dqdot = aba_partial_dv_;
dqddot_dtau = aba_partial_dtau_;

// the only constraint of interest is the equality constraint stating that the robot cannot have velocity in its own lateral direction
// ie. the no lateral slip condition, local_base_y_vel = 0.

// local base velocity in y direction is computed as follows
computeJointJacobian(Model_, Data_, q_, base_idx_, local_base_J)
CONSTR = local_base_J.row(1) * qdot_;

// derivative of this joint velocity with respect to state:
computeForwardKinematicsDerivatives(Model_, Data, q_, qdot_ Eigen::Matrix<scalar_t, GENERALIZED_VEL_NUM_, 1>::Zero());
getJointVelocityDerivatives(Model_, Data, base_idx_, LOCAL, v_partial_dq, v_partial_dv);

dCONSTR_dq = v_partial_dq.row(1);
dCONSTR_dqdot = v_partial_dv.row(1);

Thank you in advance for your reply. Kindest regards, Michael.

[jcarpent]

@micko-plz

1. With the torque vector I am giving to the forward dynamics function aba, can I ensure the force being applied along the lateral 'y' direction of the robot's base is zero?

This is possible, but not trivial. You should formulate a QP problem for that.

Indeed, you have to solved an equality constrained problems in your context. In other words, choosing the joint acceleration a will change the contact forces and vice-versa. The two quantities are coupled.

I can help you with it, but I think it would much simpler to discuss it an private thread, and if you can write a simple latex document with the problem you aim to solve it would be perfect.

@micko-plz Can you do that?

[micko-plz]

Hi @jcarpent,

Thank you for your reply. I had misunderstood what frame the vector of torques acted in. The torques regarding the base which I supplied to the forward dynamics function were acting in the world frame, rather than the base frame, something which I should have noticed given the definition of the generalized coordinate vector. I have everything working for now, but thank you for your kind offer of help. I'll close the issue.

Regards, Michael

[jcarpent]

OK. Good that things work well for you now. Best continuation!