Constraint Dynamics Drift

[jameswzhu]

I am trying to simulate a quadruped robot using the constraintDynamics function on pinocchio3. However, there sometimes seems to be drift where the feet sink into the ground. Is there a straightforward way to add in something like constraint stabilization to fix this?

[fabinsch]

Hello, can you provide some code snippet please to understand how you are using the constraintDynamics and how you implement the constraints? In my opinion, you should use the BaumgarteCorrectorParameters Kp and Kd to stabilize the constraint. Have a look at the contraint-dynamics unittest for example: https://github.com/stack-of-tasks/pinocchio/blob/pinocchio3-preview/unittest/constrained-dynamics.cpp#L912 .

[jameswzhu]

Thanks, I will look into that. Here's what I'm doing now; looping over this section of code that integrates the current dynamics and checks if a new mode has been entered based on touchdown or liftoff conditions.

        # get state and velocity vectors
        current_q = current_state[0:19]
        current_v = current_state[19:]

        # get feedforward inputs and mode
        current_input = self.horizon_inputs_[ii]

        # compute accelerations
        a = pin.constraintDynamics(self.model_,self.data_sim_,current_q,current_v,current_tau,self.contact_models_,self.contact_datas_,self.prox_settings_)

        # dynamics jacobians
        A_mat, B_mat = self.compute_dynamics_matrices(current_state, current_input, current_mode)

        # compute velocities and states
        new_v = current_v + a * self.dt_
        new_q = pin.integrate(self.model_,current_q,new_v*self.dt_)

        #check guards
        new_mode = self.compute_mode_update(current_mode,new_q,new_v,current_tau)

        # update dynamics matrices for use in backwards pass
        A_list.append(A_mat)
        B_list.append(B_mat)

        # apply reset if mode change (state stored as pre-impact state for ease in backwards pass)
        if new_mode != current_mode:
            self.contact_models_, self.contact_datas_, J_constraint, new_q, new_v, Xi = self.compute_constraint_dynamics(np.concatenate((new_q, new_v), axis=None), current_input, current_mode, new_mode)
            new_v = pin.impulseDynamics(self.model_,self.data_sim_,new_q,new_v,J_constraint,0,0)
            pin.initConstraintDynamics(self.model_,self.data_sim_,self.contact_models_)
            hybrid_count += 1

        # update current state
        current_state = np.concatenate((new_q, new_v), axis=None)
        current_mode = new_mode

[jcarpent]

Please share all your code if you expect help on our side.

[jcarpent]

I'm closing the issue due to the lack of activity.