def print_data(self): """ Print debug data """ print("Time: %s" % self.t) print("Cartesian: %s" % self.state['p']) print("Euler angles: %s" % rotations.quat2euler(self.state['q'])) print("Max gen. torques: %s" % self.max_torques.CurrentMax()) print("Max forces: %s" % self.max_forces.CurrentMax()) print("feet locations: %s" % self.feet_locations) print("contacts: %s" % self.contacts) print("QP feet forces: %s" % self.foot_forces) print("Joint torques: %s" % self.torques) print('\n')
def Update(self,coordinates, feet_locations, active_feet, o_ref, rpy_ref, f_prev, woof_config, qp_config, verbose = 0): """ Run the QP Balance controller """ (xyz, v_xyz, orientation, w_rpy, qpos_joints) = coordinates ########## Generate desired body accelerations ########## rpy = rotations.quat2euler(orientation) rotmat = rotations.quat2mat(orientation) # Foot Kinematics feet_locations = LegForwardKinematics(orientation, qpos_joints) # Feet contact contacts = active_feet # Use a whole-body PD controller to generate desired body moments ### Cartesian force ### wn_cart = 20 # desired natural frequency zeta_cart = 0.8 # desired damping ratio kp_cart = wn_cart**2 # wn^2 kd_cart = 2*wn_cart*zeta_cart # 2wn*zeta a_xyz = PropController( xyz, o_ref, kp_cart) + \ PropController( v_xyz, 0, kd_cart) a_xyz += np.array([0,0,9.81]) f_xyz = woof_config.MASS * a_xyz ### Angular moment ### wn_ang = 20 zeta_ang = 0.8 kp_ang = wn_ang**2 kd_ang = 2*wn_ang*zeta_ang a_rpy = PropController( rpy, rpy_ref, kp_ang) + \ PropController( w_rpy, 0.0, kd_ang) tau_rpy = np.matmul(woof_config.INERTIA, a_rpy) ### Combined force and moment ### ref_wrench = np.concatenate([f_xyz, tau_rpy]) ########## Solve for foot forces ######### # Find foot forces to achieve the desired body moments feet_forces = SolveFeetForces( feet_locations, contacts, ref_wrench, f_prev, mu = qp_config.MU, alpha = qp_config.ALPHA, beta = qp_config.BETA, gamma = qp_config.GAMMA, verbose = verbose) joint_torques = np.zeros(12) for f in range(4): # Extract the foot force vector for foot i foot_force_world = feet_forces[f*3 : f*3 + 3] # Transform from world to body frames, # The negative sign makes it the force exerted by the body foot_force_body = -np.dot(rotmat.T, foot_force_world) (beta,theta,r) = tuple(qpos_joints[f*3 : f*3 + 3]) # Transform from body frame forces into joint torques joint_torques[f*3 : f*3 + 3] = np.dot(LegJacobian(beta, theta, r).T, foot_force_body) return (joint_torques, feet_forces, ref_wrench)
def euler_angles(self): e = self.quaternion return quat2euler(e)
def step(self, sim): """ Get sensor data and update state estimate and contact estimate. Then calculate joint torques for locomotion. Details: Gait controller: Looks at phase variable to determine foot placements and COM trajectory QP: Generates joint torques to achieve given desired CoM trajectory given stance feet Swing controller: Swing controller needs reference foot landing positions and phase """ ################################### State & contact estimation ################################### self.state = self.state_estimator.update(sim) self.contacts = self.contact_estimator.update(sim) # Use forward kinematics from the robot body to compute where the woofer feet are self.feet_locations = WooferDynamics.LegForwardKinematics( self.state['q'], self.state['j']) self.phase = self.gait.getPhase(self.t) self.step_phase = self.gait.getStepPhase(self.t, self.phase) self.active_feet = self.gait.feetInContact(self.phase) (self.step_locations, self.p_step_locations) = self.gait.updateStepLocations( self.state, self.step_locations, self.p_step_locations, self.phase) # ################################### Swing leg control ################################### self.swing_torques, \ self.swing_forces,\ self.swing_trajectory, \ self.foot_positions = self.swing_controller.update( self.state, self.step_phase, self.step_locations, self.p_step_locations, self.active_feet) state = np.zeros(12) state[0:3] = self.state['p'] state[3:6] = rotations.quat2euler(self.state['q']) state[6:9] = self.state['p_d'] state[9:12] = self.state['w'] # only update the mpc trajectory at rate of planning discretization if (self.i % (self.gait_planner.dt / self.dt) == 0): self.foot_hist = self.gait.constuctFutureFootHistory( self.t, self.state, self.feet_locations, self.step_locations, self.gait_planner.N, self.gait_planner.dt) self.foot_forces = self.gait_planner.update( state, self.state['j'], self.foot_hist, self.t) # update the torques via jacobian at different rate if (self.i % (self.gait_planner.dt_torque / self.dt) == 0): for i in range(4): # import pdb; pdb.set_trace() f_i = -self.foot_forces[(3 * i):(3 * i + 3)][np.newaxis].T self.mpc_torques[(3 * i):( 3 * i + 3)] = WooferDynamics.FootForceToJointTorques( f_i, self.state['j'][(3 * i):(3 * i + 3)], self.state['q'], 0)[:, 0] # Expanded version of active feet active_feet_12 = self.active_feet[[0, 0, 0, 1, 1, 1, 2, 2, 2, 3, 3, 3]] self.torques = active_feet_12 * self.mpc_torques + ( 1 - active_feet_12) * self.swing_torques # Update our record of the maximum force/torque # self.max_forces.Update(self.foot_forces) # self.max_torques.Update(self.torques) # Log stuff # self.log_data() # Step time forward self.t += self.dt self.i += 1 return self.torques