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