diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..e43b0f9 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +.DS_Store diff --git a/BasicController.py b/BasicController.py index cca148a..5fdab30 100644 --- a/BasicController.py +++ b/BasicController.py @@ -6,4 +6,4 @@ def PropController(qpos,refpos,kp): Note: the damping for a PD controller should be added to the joint in the xml file to increase the stability of the simulation """ - return kp*(refpos-qpos) \ No newline at end of file + return kp*(refpos-qpos) diff --git a/BoundGait.py b/BoundGait.py new file mode 100644 index 0000000..a2be8a5 --- /dev/null +++ b/BoundGait.py @@ -0,0 +1,111 @@ +import numpy as np +from WooferConfig import WOOFER_CONFIG +from WooferDynamics import FootSelector, CoordinateExpander + +class BoundGait: + def __init__(self, step_time, flight_time): + """ + [FR, FL, BR, BL] + + Four Phases: + Phase 1: BR/BL on ground + Phase 2: no feet on ground + Phase 3: FR/FL on ground + Phase 4: no feet on ground + repeat + Length of Phase 1/3: step_time + Length of Phase 2/4: flight_time + """ + self.step_time = step_time + self.flight_time = flight_time + + self.phase_length = 2*step_time + 2*flight_time + + def getPhase(self, t): + phase_time = t % self.phase_length + + if phase_time < self.step_time: + return 1 + elif phase_time < (self.step_time + self.flight_time): + return 2 + elif phase_time < (2*self.step_time + self.flight_time): + return 3 + else: + return 4 + + def feetInContact(self, phase): + if phase == 1: + return np.array([0, 0, 1, 1]) + elif phase == 2: + return np.zeros(4) + elif phase == 3: + return np.array([1, 1, 0, 0]) + else: + return np.zeros(4) + + def updateStepLocations(self, state, step_locations, p_step_locations, phase): + """ + uses the heuristic in eq. 33 of the MIT Cheetah 3 MPC paper + to calculate the footstep locations + + todo: update step locations based off where they actually land + + side: right leg = 1, left leg = 0 + """ + p_diff = 2.5 * state['p_d'][0:2] * self.step_time + + new_step_locations = step_locations + new_p_step_locations = p_step_locations + + if phase == 1: + # need to move the FR/FL feet + new_step_locations[0:2] = p_step_locations[0:2] + p_diff + new_step_locations[3:5] = p_step_locations[3:5] + p_diff + new_p_step_locations[6:8] = step_locations[6:8] + new_p_step_locations[9:11] = step_locations[9:11] + + elif phase == 3: + # need to move the BR/BL feet + new_step_locations[6:8] = p_step_locations[6:8] + p_diff + new_step_locations[9:11] = p_step_locations[9:11] + p_diff + new_p_step_locations[0:2] = step_locations[0:2] + new_p_step_locations[3:5] = step_locations[3:5] + + return (new_step_locations, new_p_step_locations) + + def getStepPhase(self, t, phase): + """ + returns step phase for swing leg controller + """ + phase_time = t % self.phase_length + + time_into_traj = 0 + if phase == 4: + time_into_traj = phase_time - (self.step_time + 2*self.flight_time) + elif phase == 2: + time_into_traj = phase_time - self.flight_time + return time_into_traj/self.step_time + + def constuctFutureFootHistory(self, t, state, current_foot_locations, step_locations, N, mpc_dt): + """ + + """ + foot_hist = np.zeros((N, 12)) + + future_step_locations = step_locations - CoordinateExpander(state['p']) + + t_i = t + phase_i = self.getPhase(t_i) + prev_phase = phase_i + foot_hist[0, :] = FootSelector(self.feetInContact(phase_i)) * current_foot_locations + + for i in range(1,N): + if(phase_i != prev_phase): + foot_hist[i, :] = FootSelector(self.feetInContact(phase_i)) * future_step_locations + else: + foot_hist[i, :] = foot_hist[i-1, :] + prev_phase = phase_i + t_i += mpc_dt + phase_i = self.getPhase(t_i) + + return foot_hist diff --git a/GaitPlanner.py b/GaitPlanner.py deleted file mode 100644 index c033eb4..0000000 --- a/GaitPlanner.py +++ /dev/null @@ -1,126 +0,0 @@ -import numpy as np -from math import pi, cos, sin - -""" -Note: -Order of legs is: FR, FL, BR, BL -""" - -class GaitPlanner: - """ - Takes desired velocity and current state and outputs foot placements, CoM trajectory, and gait phase. - """ - def update(self, state, contacts, t): - """ - Output CoM trajectory and foot placements - """ - raise NotImplementedError - return (None, None, None, None, None, None) # Foot placements, CoM ref position, body ref orientation (euler), active_feet, phase - -class StandingPlanner(GaitPlanner): - """ - Stands still! - """ - def update(self, state, contacts, t): - freq = 1.0 - phase = t * 2 * pi * freq - p_ref = np.array([ sin(phase)*0.00, - cos(phase)*0.00, - sin(phase)*0.00 + 0.32]) - rpy_ref = np.array([ sin(phase)*15*pi/180.0, - cos(phase)*25*pi/180.0 + 0*pi/180, - sin(phase)*0*pi/180.0]) - - # Boolean representation of which feet the QP controller treats as in contact with the ground. - # A 1 in the ith slot means that the ith leg is in contact with the ground - active_feet = np.array([1,1,1,1]) - - return (None, None, p_ref, rpy_ref, active_feet, phase) - -class StepPlanner(GaitPlanner): - """ - Plans two walking-trot steps forward. During the first half of the stride, - the front-left and back-right legs are planned to be in stance. During the - second half of the stride, the front-right and back-left legs are planned - to be in stance. After the full stride, all legs are planned for stance. - """ - - def update(self, state, contacts, t, woof_config, gait_config): - """ - STEP_LENGTH: step length in meters - D: duration of the total two-step move in seconds - - phase: 0->0.5 & step_phase 0->1: moving FR and BL forward - phase: 0.5 -> 1 & step_phase 0->1: moving FL and BR forward - - Note that the foot placements vector is only used as reference positions for the swing controller - which means that the reference foot placements are meaningless for legs in contact - """ - - # stride starts at phase = 0 and ends at phase = 1 - phase = t/gait_config.D - - foot_height = woof_config.FOOT_RADIUS - - foot_locs = np.array([ woof_config.LEG_FB, -woof_config.LEG_LR, foot_height, - woof_config.LEG_FB, woof_config.LEG_LR, foot_height, - -woof_config.LEG_FB, -woof_config.LEG_LR, foot_height, - -woof_config.LEG_FB, woof_config.LEG_LR, foot_height]) - p_foot_locs = foot_locs - - active_feet = np.array([1,1,1,1]) - - step_phase = 0 - - if phase >= 0 and phase < 0.5: - # Move FR and BL forward - foot_locs = np.array([ woof_config.LEG_FB + gait_config.STEP_LENGTH, -woof_config.LEG_LR, foot_height, - woof_config.LEG_FB, woof_config.LEG_LR, foot_height, - -woof_config.LEG_FB, -woof_config.LEG_LR, foot_height, - -woof_config.LEG_FB + gait_config.STEP_LENGTH, woof_config.LEG_LR, foot_height]) - - p_foot_locs = np.array([ woof_config.LEG_FB, -woof_config.LEG_LR, foot_height, - woof_config.LEG_FB, woof_config.LEG_LR, foot_height, - -woof_config.LEG_FB, -woof_config.LEG_LR, foot_height, - -woof_config.LEG_FB, woof_config.LEG_LR, foot_height]) - active_feet = np.array([0,1,1,0]) - - step_phase = phase * 2.0 - - elif phase >= 0.5 and phase < 1.0: - # Move FL and BR forward - foot_locs = np.array([ woof_config.LEG_FB + gait_config.STEP_LENGTH, -woof_config.LEG_LR, foot_height, - woof_config.LEG_FB + gait_config.STEP_LENGTH, woof_config.LEG_LR, foot_height, - -woof_config.LEG_FB + gait_config.STEP_LENGTH, -woof_config.LEG_LR, foot_height, - -woof_config.LEG_FB + gait_config.STEP_LENGTH, woof_config.LEG_LR, foot_height]) - p_foot_locs = np.array([ woof_config.LEG_FB + gait_config.STEP_LENGTH, -woof_config.LEG_LR, foot_height, - woof_config.LEG_FB, woof_config.LEG_LR, foot_height, - -woof_config.LEG_FB, -woof_config.LEG_LR, foot_height, - -woof_config.LEG_FB + gait_config.STEP_LENGTH, woof_config.LEG_LR, foot_height]) - active_feet = np.array([1,0,0,1]) - - step_phase = (phase - 0.5) * 2.0 - - elif phase >= 1.0: - # All feet are forward - foot_locs = np.array([ woof_config.LEG_FB + gait_config.STEP_LENGTH, -woof_config.LEG_LR, foot_height, - woof_config.LEG_FB + gait_config.STEP_LENGTH, woof_config.LEG_LR, foot_height, - -woof_config.LEG_FB + gait_config.STEP_LENGTH, -woof_config.LEG_LR, foot_height, - -woof_config.LEG_FB + gait_config.STEP_LENGTH, woof_config.LEG_LR, foot_height]) - p_foot_locs = foot_locs - - active_feet = np.array([1,1,1,1]) - - step_phase = 0.0 - - # Want body to be level during the step - rpy_ref = np.array([0,0,0]) - - # Want the body to move forward one step length - CoM_x = gait_config.STEP_LENGTH * np.clip(phase,0,1) - p_ref = np.array([CoM_x, 0, woof_config.LEG_L]) - - # print("foot placements:") - # print(foot_locs) - - return (foot_locs, p_foot_locs, p_ref, rpy_ref, active_feet, phase, step_phase) diff --git a/MPCPlanner.py b/MPCPlanner.py new file mode 100644 index 0000000..1ec8482 --- /dev/null +++ b/MPCPlanner.py @@ -0,0 +1,118 @@ +import numpy as np +from WooferDynamics import * +from TrajectoryGeneration import TrajectoryGeneration +import rotations + +class MPCPlanner: + def __init__(self, N, dt_plan): + self.N = N + self.dt = dt_plan + +class MPCWalkingPlanner(MPCPlanner): + def __init__(self, N, dt_plan, dt_torque, gait, init_state, desired_velocity): + self.dt = dt_plan + self.dt_torque = dt_torque + self.N = N + self.gait = gait + self.trajectoryGenerator = TrajectoryGeneration(gait, dt_plan, N) + self.init_state = init_state + self.desired_velocity = desired_velocity + + def update(self, state, qpos_joints, foot_locations, t): + referenceTrajectory = self.generateReferenceTrajectory(state, t) + + U_horizon = self.trajectoryGenerator.solveSystem(referenceTrajectory, foot_locations, t) + + feet_forces = U_horizon[:12] + feet_forces[np.abs(feet_forces) < 1e-5] = 0 + + # joint_torques = np.zeros(12) + # + # rotmat = rotations.euler2mat(state[3:6]) + # + # 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) + + # print("Feet forces: ", feet_forces) + # print("Angular velocity: ", state[9:12]) + + # return joint_torques + + return feet_forces + + def generateReferenceTrajectory(self, state, t): + scale = np.linspace(0, 1, self.N)[:, np.newaxis] + curr = np.tile(state[np.newaxis, :], (self.N, 1)) + goal = np.zeros((self.N, 12)) + goal[:, 6:8] = self.desired_velocity[:2] + + ref = np.zeros((self.N, 12)) + # Interpolate x and y velocities to the desired velocity + ref[:, 6:8] = (1 - scale) * curr[:, 6:8] + scale * goal[:, 6:8] + # Calculate reference x and y positions by integration + ref_vel = ref[:, 6:8].copy() + ref_vel *= self.dt + dxy = np.cumsum(ref_vel, axis=0) + ref[:, :2] = curr[:, :2] + dxy + # Interpolate angle positions to 0 + ref[:, 3:6] = (1 - scale) * curr[:, 3:6] + # Interpolate z position to initial state + ref[:, 2] = (1 - scale)[:, 0] * curr[:, 2] + scale[:, 0] * self.init_state[2] + # Set z, angle velocities to 0 + # They are already 0 + return ref + +class MPCOrientationPlanner(MPCWalkingPlanner): + def __init__(self, N, dt_plan, gait, init_state): + self.dt = dt_plan + self.N = N + self.gait = gait + self.trajectoryGenerator = TrajectoryGeneration(gait, dt_plan, N) + self.init_state = init_state + + def generateReferenceTrajectory(self, state, t): + ROLL_CONSTANT = 0.05 + PITCH_CONSTANT = 0.0 + YAW_CONSTANT = 0 + w = 2 * np.pi * 10 + + scale = np.linspace(0, 1, self.N)[:, np.newaxis] + curr = np.tile(state[np.newaxis, :], (self.N, 1)) + goal = np.zeros((self.N, 12)) + + ref = np.zeros((self.N, 12)) + # Interpolate x and y velocities to the desired velocity + ref[:, 6:8] = (1 - scale) * curr[:, 6:8] + scale * goal[:, 6:8] + # Calculate reference x and y positions by integration + ref_vel = ref[:, 6:8].copy() + ref_vel *= self.dt + dxy = np.cumsum(ref_vel, axis=0) + ref[:, :2] = curr[:, :2] + dxy + # Interpolate angle positions to 0 + ref[:, 3:6] = (1 - scale) * curr[:, 3:6] + # Interpolate z position to initial state + ref[:, 2] = (1 - scale)[:, 0] * curr[:, 2] + scale[:, 0] * self.init_state[2] + # Set z, angle velocities to 0 + # They are already 0 + + times = np.linspace(t + self.dt, t + (self.N - 1) * self.dt, self.N-1) + ref[1:, 3] = ROLL_CONSTANT * np.sin(w * times) + ref[1:, 4] = PITCH_CONSTANT * np.sin(w * times) + ref[1:, 5] = YAW_CONSTANT * np.sin(w * times) + ref[1:, 9] = w * ROLL_CONSTANT * np.cos(w * times) + ref[1:, 10] = w * PITCH_CONSTANT * np.cos(w * times) + ref[1:, 11] = w * YAW_CONSTANT * np.cos(w * times) + + # if t > 2.0: + # import pdb; pdb.set_trace() + + return ref diff --git a/PlotData.py b/PlotData.py deleted file mode 100644 index 94f5df8..0000000 --- a/PlotData.py +++ /dev/null @@ -1,39 +0,0 @@ -import matplotlib.pyplot as plt -import numpy as np -import pickle - -### Init graphics ### -infile = open('woofer_logs.pickle','rb') -data = pickle.load(infile) -infile.close() - - -fig, (ax1, ax2,ax3,ax4,ax5) = plt.subplots(nrows=5, sharex=True) -# fig, (ax1, ax2,ax3,ax4,ax5,ax6) = plt.subplots(nrows=6, sharex=True) - -ax1.plot(data['torque_history'].T, linewidth=0.5) -ax1.set_title('torque_history') - -ax2.plot(data['force_history'].T,linewidth=0.5) -ax2.set_title('force_history') - -ref = ax3.plot(data['ref_wrench_history'].T,linewidth=0.5) -ax3.set_title('ref_wrench_history') -ax3.legend(ref, ('x','y','z','r','p','y')) - -ax4.plot(np.array([1,1.05,1.1,1.15])*data['contacts_history'].T,linewidth=0.5) -ax4.set_title('contacts_history') - -ax5.plot(np.array([1,1.05,1.1,1.15])*data['active_feet_history'].T,linewidth=0.5) -ax5.set_title('active_feet_history') - -# ax6.plot(smooth_contacts_history.T,linewidth=0.5) - - -fig2, (ax1,ax2,ax3,ax4) = plt.subplots(nrows = 4,sharex = True) -ax1.plot(data['swing_force_history'].T, linewidth=0.5) -ax2.plot(data['swing_trajectory'].T, linewidth=0.5) -ax3.plot(data['phase_history'].T, linewidth=0.5) -ax4.plot(data['step_phase_history'].T, linewidth=0.5) - -plt.show() \ No newline at end of file diff --git a/QPBalanceController.py b/QPBalanceController.py deleted file mode 100644 index 3623ba0..0000000 --- a/QPBalanceController.py +++ /dev/null @@ -1,88 +0,0 @@ -from BasicController import PropController -import rotations -from WooferDynamics import * -from WooferQP import SolveFeetForces - -class QPBalanceController: - def __init__(self): - self.max_forces = 0 - self.max_gen_torques = 0 - - 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) \ No newline at end of file diff --git a/SimulateWoofer.py b/SimulateWoofer.py index 58220c4..b5d0cec 100644 --- a/SimulateWoofer.py +++ b/SimulateWoofer.py @@ -7,7 +7,7 @@ """ Initailize MuJoCo -""" +""" WooferXMLParser.Parse() model = load_model_from_path("woofer_out.xml") sim = MjSim(model) @@ -28,7 +28,7 @@ # Latency options latency = WOOFER_CONFIG.LATENCY # ms of latency between torque computation and application at the joint control_rate = WOOFER_CONFIG.UPDATE_PERIOD # ms between updates (not in Hz) -tau_history = np.zeros((12,timesteps)) +tau_history = np.zeros((12,timesteps)) tau_noise = WOOFER_CONFIG.JOINT_NOISE # Nm for i in range(timesteps): @@ -36,9 +36,9 @@ tau = woofer.step(sim) tau_history[:,i] = tau - if i%50 == 0: - # pass - woofer.print_data() + # if i%50 == 0: + # # pass + # woofer.print_data() # Run the control law according to the control rate if i%control_rate == 0: @@ -54,4 +54,4 @@ Save data to file """ woofer.save_logs() -print("Done Saving Logs") \ No newline at end of file +print("Done Saving Logs") diff --git a/StandingGait.py b/StandingGait.py new file mode 100644 index 0000000..170e94b --- /dev/null +++ b/StandingGait.py @@ -0,0 +1,27 @@ +import numpy as np +from WooferConfig import WOOFER_CONFIG +from WooferDynamics import FootSelector, CoordinateExpander + +class StandingGait: + def getPhase(self, t): + return 1 + + def feetInContact(self, phase): + return np.ones(4) + + def updateStepLocations(self, state, step_locations, p_step_locations, phase): + return (step_locations, p_step_locations) + + def getStepPhase(self, t, phase): + """ + returns step phase for swing leg controller + """ + return 0 + + def constuctFutureFootHistory(self, t, state, current_foot_locations, step_locations, N, mpc_dt): + """ + + """ + foot_hist = np.tile(current_foot_locations, (N,1)) + + return foot_hist diff --git a/SwingLegController.py b/SwingLegController.py index 6c45eb4..0047df5 100644 --- a/SwingLegController.py +++ b/SwingLegController.py @@ -1,6 +1,7 @@ import numpy as np import WooferDynamics from math import pi, sin +from WooferConfig import WOOFER_CONFIG, SWING_CONTROLLER_CONFIG class SwingLegController: def update(self, state, contacts, step_phase): @@ -18,7 +19,8 @@ class PDSwingLegController(SwingLegController): Computes joint torques to bring the swing legs to their new locations """ - def trajectory(self, state, step_phase, step_locs, p_step_locs, active_feet, swing_config): + + def trajectory(self, state, step_phase, step_locs, p_step_locs, active_feet): """ sin2-based parametric trajectory planner @@ -33,7 +35,7 @@ def trajectory(self, state, step_phase, step_locs, p_step_locs, active_feet, swi # foot heights foot_heights = np.zeros(12) - foot_heights[[2,5,8,11]] = swing_config.STEP_HEIGHT * (sin(step_phase * pi))**2 + foot_heights[[2,5,8,11]] = SWING_CONTROLLER_CONFIG.STEP_HEIGHT * (sin(step_phase * pi))**2 # combine ground plane interp and foot heights swing_foot_reference_p = WooferDynamics.FootSelector(1-active_feet)*(ground_plane_foot_reference + foot_heights) @@ -44,13 +46,13 @@ def trajectory(self, state, step_phase, step_locs, p_step_locs, active_feet, swi return swing_foot_reference_p - def update(self, state, step_phase, step_locs, p_step_locs, active_feet, woof_config, swing_config): - reference_positions = self.trajectory(state, step_phase, step_locs, p_step_locs, active_feet, swing_config) + def update(self, state, step_phase, step_locs, p_step_locs, active_feet): + reference_positions = self.trajectory(state, step_phase, step_locs, p_step_locs, active_feet) actual_positions = WooferDynamics.FootLocationsWorld(state) errors = reference_positions - actual_positions - foot_forces = WooferDynamics.CoordinateExpander(swing_config.KP) * errors * WooferDynamics.FootSelector(1-active_feet) + foot_forces = WooferDynamics.CoordinateExpander(SWING_CONTROLLER_CONFIG.KP) * errors * WooferDynamics.FootSelector(1-active_feet) leg_torques = np.zeros(12) for i in range(4): @@ -59,10 +61,6 @@ def update(self, state, step_phase, step_locs, p_step_locs, active_feet, woof_co specific_leg_joints = state['j'][3*i:3*i+3] leg_torques[3*i:3*i+3] = WooferDynamics.FootForceToJointTorques(specific_foot_force, specific_leg_joints, state['q'], abaduction_offset = 0) - # print(leg_torques, reference_positions) + # print("Reference world positions: ", reference_positions) # print(" ") return leg_torques, foot_forces, reference_positions, actual_positions - - - - diff --git a/TrajectoryGeneration.py b/TrajectoryGeneration.py new file mode 100644 index 0000000..9be96d6 --- /dev/null +++ b/TrajectoryGeneration.py @@ -0,0 +1,236 @@ +from scipy.signal import cont2discrete +import numpy as np +from math import cos, sin +from WooferConfig import WOOFER_CONFIG +from MathUtils import CrossProductMatrix +import cvxpy as cvx +import pdb + +class TrajectoryGeneration: + def __init__(self, gait, dt, N): + """ + insert stuff + """ + self.J = WOOFER_CONFIG.INERTIA + self.m = WOOFER_CONFIG.MASS # 7.172 + # self.m = 7.172 + + self.C = np.zeros((1,13)) + self.D = np.zeros((1,12)) + + self.gait = gait + + # planning horizon + self.N = N + + # discretization step of planning horizon + self.dt = dt + + self.Q = np.eye(13) + + # tuning params + + self.Q[0,0] = 1 + self.Q[1,1] = 1 + + # z position + # self.Q[2,2] = 100 + + # roll + # self.Q[3,3] = 4 + + # pitch + # self.Q[3,3] = 10 + + # yaw = + # self.Q[5,5] = 100 + + self.alpha = 1e-6 + self.R = self.alpha*np.eye(12) + + self.mu = 1.5 + + self.max_horz_force = 133 + self.max_vert_force = 133 + self.min_vert_force = 1 + + def rotZ(self, yaw): + Rz = np.zeros((3,3)) + + Rz[0,0] = cos(yaw) + Rz[0,1] = sin(yaw) + Rz[1,0] = -sin(yaw) + Rz[1,1] = cos(yaw) + Rz[2,2] = 1 + + return Rz + + def constructA(self, yaw): + """ + constructs the continuous time A matrix as a function of yaw + """ + A = np.zeros((13, 13)) + Rz = self.rotZ(yaw) + + A[0:3, 6:9] = np.eye(3) + A[3:6, 9:12] = Rz + A[8, 12] = 1 + + return A + + def constructB(self, foot_locations, yaw): + """ + constructs the continuous time B matrix as a function of yaw and foot locations + """ + B = np.zeros((13, 12)) + Rz = self.rotZ(yaw) + + J_w_inv = np.linalg.inv(Rz @ self.J @ Rz.T) + R1 = CrossProductMatrix(foot_locations[0:3]) + R2 = CrossProductMatrix(foot_locations[3:6]) + R3 = CrossProductMatrix(foot_locations[6:9]) + R4 = CrossProductMatrix(foot_locations[9:12]) + + B[6:9, 0:3] = np.eye(3)/self.m + B[6:9, 3:6] = np.eye(3)/self.m + B[6:9, 6:9] = np.eye(3)/self.m + B[6:9, 9:12] = np.eye(3)/self.m + + B[9:12, 0:3] = J_w_inv @ R1 + B[9:12, 3:6] = J_w_inv @ R2 + B[9:12, 6:9] = J_w_inv @ R3 + B[9:12, 9:12] = J_w_inv @ R4 + + return B + + def getDiscrete(self, A, B): + """ + get ZOH approximation of A and B matrices + """ + (A_d, B_d, _, _, _) = cont2discrete((A, B, self.C, self.D), self.dt) + + return A_d, B_d + + def solveSystem(self, refTrajectory, foot_hist, t): + """ + build matrices and call solver + + refTrajectory (of length N): + [pos, euler orientation, velocity, angular velocity] + """ + assert(refTrajectory.shape[0] == self.N) + yaw_bar = np.mean(refTrajectory[:, 3]) + + # if t > 1.0: + # import pdb; pdb.set_trace() + + A = self.constructA(yaw_bar) + A_hat = None + B_hats = np.zeros((self.N, 13, 12)) + + curr_t = t + + for i in range(self.N): + # active_feet = self.gait.feetInContact(self.gait.getPhase(curr_t)) + # active_feet_12 = active_feet[[0,0,0,1,1,1,2,2,2,3,3,3]] + + foot_loc_i = foot_hist[i,:] + + B_i = self.constructB(foot_loc_i, refTrajectory[i, 3]) + + A_d, B_d = self.getDiscrete(A, B_i) + if i == 0: A_hat = A_d + B_hats[i] = B_d + + curr_t += self.dt + + # if t > 0.1: + # import pdb; pdb.set_trace() + C, c_up = self.makeConstraints(t) + prob, u = self.build_qp(refTrajectory, A_hat, B_hats, C, c_up) + + prob.solve(solver=cvx.OSQP) + return u.value + + + def makeConstraints(self, t): + """ + create constraint matrix C and lower and upper bounds + + need to check these + """ + curr_t = t + + C = np.zeros((self.N, 24, 12)) + c_up = np.zeros((self.N, 24)) + + fz_mat = np.zeros((4, 12)) + fx_mat = np.zeros((8, 12)) + fy_mat = np.zeros((8, 12)) + for i in range(4): + fz_mat[i, 3*i + 2] = 1 + + fx_mat[2*i, 3*i] = 1 + fx_mat[2*i, 3*i + 2] = -self.mu + fx_mat[2*i + 1, 3*i] = -1 + fx_mat[2*i + 1, 3*i + 2] = -self.mu + + fy_mat[2*i, 3*i + 1] = 1 + fy_mat[2*i, 3*i + 2] = -self.mu + fy_mat[2*i + 1, 3*i + 1] = -1 + fy_mat[2*i + 1, 3*i + 2] = -self.mu + + for i in range(self.N): + foot_loc_i = self.gait.feetInContact(self.gait.getPhase(curr_t)) + # Z Force constraints, accounting for which feet are grounded + c_up[i, :4] = foot_loc_i * self.max_vert_force + C[i, :4] = fz_mat + c_up[i, 4:8] = -foot_loc_i * self.min_vert_force + C[i, 4:8] = -fz_mat + # X Force constraints, accounting for which feet are grounded + C[i, 8:16] = fx_mat + # Y Force constraints, accounting for which feet are grounded + C[i, 16:24] = fy_mat + + curr_t += self.dt + + return C, c_up + + + def build_qp(self, x_ref, A_hat, B_hats, constr, c_up): + # n number of state variables + # k number of horizon state points + # m dimension of control input at each point + + x_ref = x_ref.T + x_ref = np.concatenate((x_ref, -9.81*np.ones((1, self.N)))) + n, k = x_ref.shape + k = self.N + m = B_hats.shape[2] + + D = np.zeros((k * n, n)) + for i in range(k): + D[i * n: (i + 1) * n, :] = np.linalg.matrix_power(A_hat, i) + + C = np.zeros((k * n, m * k)) + for i in range(k): + for j in range(i): + C[i * n: (i + 1) * n, j * m: (j + 1) * m] = \ + np.linalg.matrix_power(A_hat, (i - 1)) @ B_hats[j] + + Qs = np.zeros((n * k, n * k)) + Rs = np.zeros((m * k, m * k)) + for i in range(k): + Qs[i * n: (i + 1) * n, i * n: (i + 1) * n] = self.Q + Rs[i * m: (i + 1) * m, i * m: (i + 1) * m] = self.R + + u = cvx.Variable(m * k) + target = x_ref.T.flatten() - D @ x_ref[:, 0] + cost = cvx.quad_form(C * u - target, Qs) + cvx.quad_form(u, Rs) + obj = cvx.Minimize(cost) + constraints = [] + for i in range(self.N): + constraints += [constr[i] * u[m*i:m*(i+1)] <= c_up[i]] + prob = cvx.Problem(obj, constraints) + + return prob, u diff --git a/TrotGait.py b/TrotGait.py new file mode 100644 index 0000000..9210da8 --- /dev/null +++ b/TrotGait.py @@ -0,0 +1,126 @@ +import numpy as np +from WooferConfig import WOOFER_CONFIG +from WooferDynamics import FootSelector, CoordinateExpander + +class TrotGait: + def __init__(self, step_time, overlap_time): + """ + [FR, FL, BR, BL] + + Four Phases: + Phase 1: All feet on ground + Phase 2: FR/BL on ground + Phase 3: All feet on ground + Phase 4: FL/BR on ground + repeat + Length of Phase 1/3: step_time + Length of Phase 2/4: overlap_time + """ + self.step_time = step_time + self.overlap_time = overlap_time + + self.isFirstStep = 1 + + self.phase_length = 2*step_time + 2*overlap_time + + def getPhase(self, t): + phase_time = t % self.phase_length + + if phase_time < self.overlap_time: + return 1 + elif phase_time < (self.step_time + self.overlap_time): + return 2 + elif phase_time < (self.step_time + 2*self.overlap_time): + return 3 + else: + return 4 + + def feetInContact(self, phase): + if phase == 1: + return np.ones(4) + elif phase == 2: + return np.array([1, 0, 0, 1]) + elif phase == 3: + return np.array([1, 1, 1, 1]) + else: + return np.array([0, 1, 1, 0]) + + def updateStepLocations(self, state, step_locations, p_step_locations, phase): + """ + uses the heuristic in eq. 33 of the MIT Cheetah 3 MPC paper + to calculate the footstep locations + + todo: update step locations based off where they actually land + (right now it assumes that foot locations end up close to desired) + + side: right leg = 1, left leg = 0 + """ + p_diff = 2.625 * state['p_d'][0:2] * self.step_time + + # if self.isFirstStep: + # p_diff = np.array([0.15, 0.0]) + # if phase == 2: + # self.isFirstStep = 0 + # else: + # p_diff = np.array([0.315, 0.0]) + + # p_diff = np.zeros(2) + + new_step_locations = step_locations + new_p_step_locations = p_step_locations + + if phase == 1: + # need to move the FL/BR feet + new_step_locations[3:5] = p_step_locations[3:5] + p_diff + new_step_locations[6:8] = p_step_locations[6:8] + p_diff + new_p_step_locations[0:2] = step_locations[0:2] + new_p_step_locations[9:11] = step_locations[9:11] + + elif phase == 3: + # need to move the FR/BL feet + new_step_locations[0:2] = p_step_locations[0:2] + p_diff + new_step_locations[9:11] = p_step_locations[9:11] + p_diff + new_p_step_locations[3:5] = step_locations[3:5] + new_p_step_locations[6:8] = step_locations[6:8] + + return (new_step_locations, new_p_step_locations) + + def getStepPhase(self, t, phase): + """ + returns step phase for swing leg controller + """ + phase_time = t % self.phase_length + + time_into_traj = 0 + if phase == 4: + time_into_traj = phase_time - (self.step_time + 2*self.overlap_time) + elif phase == 2: + time_into_traj = phase_time - self.overlap_time + return time_into_traj/self.step_time + + def constuctFutureFootHistory(self, t, state, current_foot_locations, step_locations, N, mpc_dt): + """ + + """ + foot_hist = np.zeros((N, 12)) + + + # doesn't really make sense - probably should be based off of reference trajectory + # should move this function to MPCPlanner class + future_step_locations = step_locations - CoordinateExpander(state['p']) + + t_i = t + phase_i = self.getPhase(t_i) + prev_phase = phase_i + foot_hist[0, :] = FootSelector(self.feetInContact(phase_i)) * current_foot_locations + + for i in range(1,N): + if(phase_i != prev_phase): + foot_hist[i, :] = FootSelector(self.feetInContact(phase_i)) * future_step_locations + else: + foot_hist[i, :] = foot_hist[i-1, :] + prev_phase = phase_i + t_i += mpc_dt + phase_i = self.getPhase(t_i) + + return foot_hist diff --git a/WooferConfig.py b/WooferConfig.py index 1a09439..7e0cc2d 100644 --- a/WooferConfig.py +++ b/WooferConfig.py @@ -30,12 +30,12 @@ def __init__(self): self.JOINT_NOISE = 0.5 # Nm, 1 sigma of gaussian noise self.LATENCY = 2 # ms of sense->control latency - self.UPDATE_PERIOD = 2 # ms between control updates + self.UPDATE_PERIOD = 1 # ms between control updates class EnvironmentConfig: def __init__(self): self.MU = 1.5 # coeff friction - self.SIM_STEPS = 1500 # simulation steps to take + self.SIM_STEPS = 5000 # simulation steps to take self.DT = 0.001 # timestep [s] # Software stuff @@ -48,7 +48,7 @@ def __init__(self): class SwingControllerConfig: def __init__(self): - self.STEP_HEIGHT = 0.08 # [m] + self.STEP_HEIGHT = 0.1 # [m] self.KP = np.array([400,400,2000]) # [Nm/rad, Nm/rad, N/m] class GaitPlannerConfig: @@ -60,4 +60,4 @@ def __init__(self): QP_CONFIG = QPConfig() SWING_CONTROLLER_CONFIG = SwingControllerConfig() GAIT_PLANNER_CONFIG = GaitPlannerConfig() -ENVIRONMENT_CONFIG = EnvironmentConfig() \ No newline at end of file +ENVIRONMENT_CONFIG = EnvironmentConfig() diff --git a/WooferQP.py b/WooferQP.py deleted file mode 100644 index 94731d9..0000000 --- a/WooferQP.py +++ /dev/null @@ -1,177 +0,0 @@ -import osqp -import scipy.sparse as sparse -import numpy as np -import math - -from MathUtils import CrossProductMatrix - -def AccelerationMatrix(q_feet): - """ - Generates the matrix that maps from foot forces to body accelerations in x,y,z and pitch,roll,yaw - From Equation (5) in the paper "High-slope terrain locomotion for torque-controlled quadruped robots" - - q_feet: 12 vector of the foot locations, in world coordinates centered around the robot COM - - Note: The 12-vector of foot forces are ground reaction forces, so for example, if the foot is pushing down, - the z-component is >= 0 - """ - A = np.zeros((6,12)) - A[0:3,0:3] = np.eye(3) - A[0:3,3:6] = np.eye(3) - A[0:3,6:9] = np.eye(3) - A[0:3,9:12] = np.eye(3) - A[3:6,0:3] = CrossProductMatrix(q_feet[0:3]) - A[3:6,3:6] = CrossProductMatrix(q_feet[3:6]) - A[3:6,6:9] = CrossProductMatrix(q_feet[6:9]) - A[3:6,9:12] = CrossProductMatrix(q_feet[9:12]) - return A - - -def SolveFeetForces(q_feet, feet_contact, reference_wrench, f_prev, mu = 1.0, alpha = 1e-3, beta = 0.1, gamma = 100, verbose=0): - """ - Use OSQP to solve for feet forces. - - q_feet : Location of feet relative to body COM in world coordinates - (yaw may be in local coordinates) - reference_wrench : reference force and torque on the body - mu : Friction coefficient - alpha : Weight on normalizer for feet forces - beta : Weight on smoothness between feet forces - gamma : Scaling factor on angular acceleration deviations - - OSQP uses an objective of the form: - (1/2)x'Px + qx - and linear ineq constraints: - lb < Cx < ub - - Our objective is to select foot forces that minimize the difference between the - realized body acceleration and a desired body acceleration. - - This is akin to: - Minimize ||Ax-b||^2 - However, we want to prioritize angular stability of the body, so we use a quadratic norm of K: - Minimize [(Ax-b)'K(Ax-b)] - - - When expanded, it becomes: - Minimize x'A'KAx - 2A'Kbx + b'Kb - We can write in the OSPQ form with the following substitutions - P = 2A'KA - q = -2A'Kb - - We also want to minimize the norm of x to minimize foot forces, and minimize the - difference between new and old foot forces so the final objective is - - Minimize x'(A'KA + R + beta*I)x - 2(A'Kb + beta*x0) - P = 2(A'KA + R + betaI) - q = - 2A'Kb - beta*x0 - - The derivation of the smoothness term is - loss = beta*(x1-x0)'(x1-x0) = beta*x1'x1 - beta*2*x0'x1 + beta*x0'x0 - - in the code A'KA is called AKA, R is alpha*I, and x0 is called f_prev (previous force) - """ - - ## Params ## - max_horz_force = 133 - max_vert_force = 133 - min_vert_force = 1 - - A = AccelerationMatrix(q_feet) - b = reference_wrench - - # Construct normalizer matrix R which minimizes total foot forces - R = np.eye(12)*alpha - - # Construct quadratic norm matrix K - K = np.diag([1,1,1,gamma,gamma,gamma]) - - # Construct AKA - AKA = np.matmul(A.T,np.matmul(K, A)) - - # Construct the final P matrix - P_dense = 2*(AKA + R + beta*np.eye(12)) - P = sparse.csc_matrix(P_dense) - - # Construct the linear term q - q = -2*np.matmul(A.T,np.matmul(K, b)) - beta*f_prev - - # Set up the inequality constraints with matrix X - mu_pyramid = mu/(2**0.5) - # cone: fx^2+fy^2 <= mu^2 * fz^2 - C = np.zeros((28,12)) - - # Enforce friction pyramid constraints on all 4 feet - for i in range(4): - # fz >= 0 - C[i*5, i*3+2] = 1 - # ufz+fx >= 0 - C[i*5+1, i*3] = 1 - C[i*5+1, i*3+2] = mu - # ufz-fx >= 0 - C[i*5+2, i*3] = -1 - C[i*5+2, i*3+2] = mu - # ufz+fy >= 0 - C[i*5+3, i*3+1] = 1 - C[i*5+3, i*3+2] = mu - # ufz-fy >= 0 - C[i*5+4, i*3+1] = -1 - C[i*5+4, i*3+2] = mu - - # Enforce limits on horizontal components - for i in range(4): - C[i*2+20, i*3] = 1 - C[i*2+20+1, i*3+1]= 1 - - C = sparse.csc_matrix(C) - lb = np.zeros((28,)) - ub = np.array([np.inf]*28) - for i in range(4): - lb[i*5] = min_vert_force if feet_contact[i] else 0 - ub[i*5] = max_vert_force if feet_contact[i] else 0 - - # Enforce that the horizontal ||foot forces|| are <= 40 - lb[20:] = -max_horz_force - ub[20:] = max_horz_force - - # print(feet_contact, ub) - - prob = osqp.OSQP() - prob.setup(P,q,C,lb,ub,alpha=1.0,verbose=0) - res = prob.solve() - - if verbose>0: - print('------------ QP Outputs ------------') - print('Desired acceleration') - print(b) - print('Foot forces:') - print(res.x) - print('Status: %s'%res.info.status) - print('Actual accleration') - print(np.dot(A,res.x)) - - - acc_cost = np.matmul(res.x, np.matmul(AKA, res.x)) + np.dot(q,res.x) + np.dot(b,b) - force_cost = np.dot(res.x, np.dot(R, res.x)) - print('Accuracy cost: %s \t Force cost: %s'%(acc_cost, force_cost)) - print('\n') - - return res.x - - -# # generate basic foot config -# # this is for testing only -# np.set_printoptions(suppress=True) -# body_w = 0.4 #[m] -# body_l = 0.8 #[m] -# stance_h = 0.6 #[m] -# xfl = np.array([-body_w/2, body_l/2, -stance_h]) -# xfr = np.array([body_w/2, body_l/2, -stance_h]) -# xbl = np.array([-body_w/2, -body_l/2, -stance_h]) -# xbr = np.array([body_w/2, -body_l/2, -stance_h]) - -# q_feet = np.concatenate((xfl,xfr,xbl,xbr)) -# # generate acceleration matrix - -# reference_wrench = np.array([0, 0, 9.81, 1, 0,0 ]) -# SolveFeetForces(q_feet, reference_wrench) diff --git a/WooferRobot.py b/WooferRobot.py index 58f2731..45f8827 100644 --- a/WooferRobot.py +++ b/WooferRobot.py @@ -7,35 +7,37 @@ from MathUtils import CrossProductMatrix, RunningMax # Provides kinematic functions among others -import WooferDynamics +import WooferDynamics from JointSpaceController import JointSpaceController, TrotPDController from BasicController import PropController -from QPBalanceController import QPBalanceController from StateEstimator import MuJoCoStateEstimator from ContactEstimator import MuJoCoContactEstimator -from GaitPlanner import StandingPlanner, StepPlanner from SwingLegController import PDSwingLegController, ZeroSwingLegController +from TrotGait import TrotGait +from StandingGait import StandingGait +from BoundGait import BoundGait +from MPCPlanner import MPCWalkingPlanner, MPCOrientationPlanner from WooferConfig import WOOFER_CONFIG, QP_CONFIG, SWING_CONTROLLER_CONFIG, GAIT_PLANNER_CONFIG class WooferRobot(): """ - This class represents the onboard Woofer software. + This class represents the onboard Woofer software. The primary input is the mujoco simulation data and the - primary output is a set joint torques. + primary output is a set joint torques. """ - def __init__(self, state_estimator, contact_estimator, qp_controller, gait_planner, swing_controller, dt): + def __init__(self, state_estimator, contact_estimator, gait, gait_planner, swing_controller, dt): """ Initialize object variables """ self.contact_estimator = contact_estimator self.state_estimator = state_estimator - self.qp_controller = qp_controller # QP controller for calculating foot forces self.gait_planner = gait_planner self.swing_controller = swing_controller + self.gait = gait self.state = None self.contacts = None @@ -47,41 +49,46 @@ def __init__(self, state_estimator, contact_estimator, qp_controller, gait_plann self.data = {} self.data['torque_history'] = np.zeros((12,init_data_size)) self.data['force_history'] = np.zeros((12,init_data_size)) - self.data['ref_wrench_history'] = np.zeros((6,init_data_size)) self.data['contacts_history'] = np.zeros((4,init_data_size)) - self.data['active_feet_history'] = np.zeros((4,init_data_size)) + self.data['active_feet_history'] = np.zeros((4,init_data_size)) self.data['swing_torque_history'] = np.zeros((12,init_data_size)) self.data['swing_force_history'] = np.zeros((12,init_data_size)) self.data['swing_trajectory'] = np.zeros((12,init_data_size)) self.data['foot_positions'] = np.zeros((12,init_data_size)) - self.data['phase_history'] = np.zeros((1,init_data_size)) - self.data['step_phase_history'] = np.zeros((1,init_data_size)) self.dt = dt self.t = 0 self.i = 0 + self.phase = 1 + self.foot_forces = np.array([0,0,WOOFER_CONFIG.MASS*9.81/4]*4) - self.phase = 0 - self.step_phase = 0 # Increases from 0 to 1 and back to 0 every step - self.step_locations = np.zeros(12) - self.p_step_locations = np.zeros(12) + self.torques = np.zeros(12) + + self.step_locations = np.array([WOOFER_CONFIG.LEG_FB, -WOOFER_CONFIG.LEG_LR, WOOFER_CONFIG.FOOT_RADIUS, + WOOFER_CONFIG.LEG_FB, WOOFER_CONFIG.LEG_LR, WOOFER_CONFIG.FOOT_RADIUS, + -WOOFER_CONFIG.LEG_FB, -WOOFER_CONFIG.LEG_LR, WOOFER_CONFIG.FOOT_RADIUS, + -WOOFER_CONFIG.LEG_FB, WOOFER_CONFIG.LEG_LR, WOOFER_CONFIG.FOOT_RADIUS]) + self.p_step_locations = np.array([WOOFER_CONFIG.LEG_FB, -WOOFER_CONFIG.LEG_LR, WOOFER_CONFIG.FOOT_RADIUS, + WOOFER_CONFIG.LEG_FB, WOOFER_CONFIG.LEG_LR, WOOFER_CONFIG.FOOT_RADIUS, + -WOOFER_CONFIG.LEG_FB, -WOOFER_CONFIG.LEG_LR, WOOFER_CONFIG.FOOT_RADIUS, + -WOOFER_CONFIG.LEG_FB, WOOFER_CONFIG.LEG_LR, WOOFER_CONFIG.FOOT_RADIUS]) self.swing_torques = np.zeros(12) self.swing_trajectory = np.zeros(12) - self.foot_positions = np.zeros(12) + self.mpc_torques = np.zeros(12) 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: + + QP: Generates joint torques to achieve given desired CoM trajectory given stance feet Swing controller: @@ -91,60 +98,65 @@ def step(self, sim): self.state = self.state_estimator.update(sim) self.contacts = self.contact_estimator.update(sim) - ################################### Gait planning ################################### - (self.step_locations, self.p_step_locations, \ - p_ref, rpy_ref, self.active_feet, self.phase, self.step_phase) = self.gait_planner.update( self.state, - self.contacts, - self.t, - WOOFER_CONFIG, - GAIT_PLANNER_CONFIG) - # print("phase: %s"%self.phase) - - ################################### Swing leg control ################################### - # TODO. Zero for now, but in the future the swing controller will provide these torques + # 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.foot_positions = self.swing_controller.update( self.state, + self.step_phase, self.step_locations, - self.p_step_locations, - self.active_feet, - WOOFER_CONFIG, - SWING_CONTROLLER_CONFIG) - - ################################### QP force control ################################### - # Rearrange the state for the qp solver - qp_state = (self.state['p'], - self.state['p_d'], - self.state['q'], - self.state['w'], - self.state['j']) + 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] - # 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']) - # Calculate foot forces using the QP solver - (qp_torques, self.foot_forces, self.ref_wrench) = self.qp_controller.Update(qp_state, - self.feet_locations, - self.active_feet, - p_ref, - rpy_ref, - self.foot_forces, - WOOFER_CONFIG, - QP_CONFIG) # Expanded version of active feet - active_feet_12 = self.active_feet[[0,0,0,1,1,1,2,2,2,3,3,3]] + active_feet_12 = self.active_feet[[0,0,0,1,1,1,2,2,2,3,3,3]] - # Mix the QP-generated torques and PD-generated torques to produce the final joint torques sent to the robot - self.torques = active_feet_12 * qp_torques + (1 - active_feet_12) * self.swing_torques + 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) + # self.max_forces.Update(self.foot_forces) + # self.max_torques.Update(self.torques) # Log stuff - self.log_data() + # self.log_data() # Step time forward self.t += self.dt @@ -155,26 +167,23 @@ def step(self, sim): def log_data(self): """ Append data to logs - """ + """ data_len = self.data['torque_history'].shape[1] if self.i > data_len - 1: for key in self.data.keys(): self.data[key] = np.append(self.data[key], np.zeros((np.shape(self.data[key])[0],1000)),axis=1) - + self.max_forces.Update(self.foot_forces) self.max_torques.Update(self.torques) self.data['torque_history'][:,self.i] = self.torques self.data['force_history'][:,self.i] = self.foot_forces - self.data['ref_wrench_history'][:,self.i] = self.ref_wrench self.data['contacts_history'][:,self.i] = self.contacts self.data['active_feet_history'][:,self.i] = self.active_feet self.data['swing_torque_history'][:,self.i] = self.swing_torques self.data['swing_force_history'][:,self.i] = self.swing_forces self.data['swing_trajectory'][:,self.i] = self.swing_trajectory self.data['foot_positions'][:,self.i] = self.foot_positions - self.data['phase_history'][:,self.i] = self.phase - self.data['step_phase_history'][:,self.i] = self.step_phase def print_data(self): """ @@ -185,7 +194,6 @@ def print_data(self): 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("Reference wrench: %s"%self.ref_wrench) print("feet locations: %s" %self.feet_locations) print("contacts: %s" %self.contacts) print("QP feet forces: %s" %self.foot_forces) @@ -204,20 +212,36 @@ def MakeWoofer(dt = 0.001): """ Create robot object """ - mujoco_state_est = MuJoCoStateEstimator() - mujoco_contact_est = MuJoCoContactEstimator() - qp_controller = QPBalanceController() - # gait_planner = StandingPlanner() - gait_planner = StepPlanner() - swing_controller = PDSwingLegController() + trot_overlap_time = .105 + trot_step_time = .3 - woofer = WooferRobot(mujoco_state_est, mujoco_contact_est, qp_controller, gait_planner, swing_controller, dt = dt) - - return woofer + bound_step_time = 0.05 + bound_flight_time = 0.01 + MPC_horizon = 16 + MPC_planning_timestep = 0.01 + MPC_torque_timestep = 0.001 + # Trot: + # MPC_desired_velocity = np.array([0.305, 0.0, 0.0]) + MPC_desired_velocity = np.array([0.3, 0, 0.0]) + # Bound + # MPC_desired_velocity = np.array([0.0, 0.0, 0.0]) + mujoco_state_est = MuJoCoStateEstimator() + mujoco_contact_est = MuJoCoContactEstimator() + # gait = BoundGait(bound_step_time, bound_flight_time) + gait = TrotGait(trot_step_time, trot_overlap_time) + # gait = StandingGait() + gait_planner = MPCWalkingPlanner(MPC_horizon, MPC_planning_timestep, MPC_torque_timestep, + gait, np.array([0, 0, (WOOFER_CONFIG.LEG_L + WOOFER_CONFIG.FOOT_RADIUS), 0, 0, 0, 0, 0, 0, 0, 0, 0]), + MPC_desired_velocity) + # gait_planner = MPCOrientationPlanner(MPC_horizon, MPC_planning_timestep, + # gait, np.array([0, 0, (WOOFER_CONFIG.LEG_L + WOOFER_CONFIG.FOOT_RADIUS), 0, 0, 0, 0, 0, 0, 0, 0, 0])) + swing_controller = PDSwingLegController() + woofer = WooferRobot(mujoco_state_est, mujoco_contact_est, gait, gait_planner, swing_controller, dt = dt) + return woofer diff --git a/qp_test.py b/qp_test.py deleted file mode 100644 index f096539..0000000 --- a/qp_test.py +++ /dev/null @@ -1,125 +0,0 @@ -import cvxpy as cp -import numpy as np - -np.set_printoptions(suppress=True) - -l1 = 0.2 #[m] - -body_w = 0.4 #[m] -body_l = 0.8 #[m] -stance_h = 0.6 #[m] - -body_m = 8.0 #[kg] -n = 4 #[num feet] - -def CrossProductMatrix(a): - return np.array([[0, -a[2], a[1]], \ - [a[2], 0, -a[0]], \ - [-a[1], a[0], 0]]) - -def AccelerationMatrix(q_feet): - """ - Generates the matrix that maps from foot forces to body accelerations in x,y,z and pitch,roll,yaw - From Equation (5) in the paper "High-slope terrain locomotion for torque-controlled quadruped robots" - - q_feet: 12-vector containing the foot reaction forces, ie if the foot is pushing down, - the z-component is >= 0 - """ - A = np.zeros((6,12)) - A[0:3,0:3] = np.eye(3) - A[0:3,3:6] = np.eye(3) - A[0:3,6:9] = np.eye(3) - A[0:3,9:12] = np.eye(3) - A[3:6,0:3] = CrossProductMatrix(q_feet[0:3]) - A[3:6,3:6] = CrossProductMatrix(q_feet[3:6]) - A[3:6,6:9] = CrossProductMatrix(q_feet[6:9]) - A[3:6,9:12] = CrossProductMatrix(q_feet[9:12]) - return A - -# generate basic foot config -# this is for testing only -xfl = np.array([-body_w/2, body_l/2, -stance_h]) -xfr = np.array([body_w/2, body_l/2, -stance_h]) -xbl = np.array([-body_w/2, -body_l/2, -stance_h]) -xbr = np.array([body_w/2, -body_l/2, -stance_h]) - -q_feet = np.concatenate((xfl,xfr,xbl,xbr)) -# generate acceleration matrix -A = AccelerationMatrix(q_feet) - -# verify A -print('Map between foot force and acceleration:') -print(A) - -##################### Four-foot Test (Stand) ######################### - -# target body acceleration -b = np.zeros((6,1)) -b[2] = 10 # [m/s2] -b[3] = 1 # [rad/s2] - -##### Construct the problem ####### -x = cp.Variable(3*n) -objective = cp.Minimize(cp.sum_squares(A*x - b) + (1e-3)*cp.norm(x)) - -# Constrain vertical component of foot forces to be >= 0 -constraints = [0 <= x[2], 0 <= x[5], 0 <= x[8], 0 <= x[11]] -prob = cp.Problem(objective, constraints) - -# Solve for foot forces -result = prob.solve() - -print('------------Four-foot test (stand)------------') -print('Desired acceleration') -print(b) -print('Foot forces:') -print(x.value) -print(prob.status) -print('Actual accleration') -print(np.dot(A,x.value)) - -##################### Two-foot Test (Trot) ######################### -# Find foot forces when you set the condition that only two legs are in contact with the ground -b = np.zeros((6,1)) -b[2] = 10 # [m/s2] -b[3] = 1 # [rad/s2] -b[4] = -1 # [rad/s2] -x = cp.Variable(3*n) -objective = cp.Minimize(cp.sum_squares(A*x - b) + (1e-3)*cp.norm(x)) -constraints = [0 <= x[2], 0 <= x[5], 0 <= x[8], 0 <= x[11]] -constraints = constraints + [0 <= x[3:6], 0 >= x[3:6], 0 <= x[6:9], 0 >= x[6:9]] -prob = cp.Problem(objective, constraints) -result = prob.solve() -print('------------Two-foot test (trot)------------') -print('Desired acceleration') -print(b) -print('Foot forces:') -print(x.value) -print(prob.status) -print('Actual accleration') -print(np.dot(A,x.value)) - -##################### Three-foot Test (Walk) ######################### -# Find foot forces when you set the condition that only two legs are in contact with the ground -b = np.zeros((6,1)) -b[2] = 10 # [m/s2] -b[3] = 1 # [rad/s2] -b[4] = -1 # [rad/s2] -x = cp.Variable(3*n) -objective = cp.Minimize(cp.sum_squares(A*x - b) + (1e-3)*cp.norm(x)) -constraints = [0 <= x[2], 0 <= x[5], 0 <= x[8], 0 <= x[11]] -constraints = constraints + [0 <= x[0:3], 0 >= x[0:3]] -prob = cp.Problem(objective, constraints) -result = prob.solve() -print('------------Three-foot test (trot)------------') -print('Desired acceleration') -print(b) -print('Foot forces:') -print(x.value) -print(prob.status) -print('Actual accleration') -print(np.dot(A,x.value)) - - - - diff --git a/trajectoryTest.py b/trajectoryTest.py new file mode 100644 index 0000000..6a77566 --- /dev/null +++ b/trajectoryTest.py @@ -0,0 +1,29 @@ +import numpy as np +from TrajectoryGeneration import TrajectoryGeneration +from TrotGait import TrotGait +from WooferConfig import WOOFER_CONFIG + + +N = 20 +discretization_step = 0.05 + +reference_trajectory = np.zeros((N, 12)) +reference_trajectory[:,0] = np.linspace(0, 1, N) +reference_trajectory[:,2] = 0.32*np.ones(N) + + +gait = TrotGait(0.5, 0.1) + +mpc = TrajectoryGeneration(gait, discretization_step, N) + + +foot_locs = np.zeros(12) +foot_locs[0:3]= np.array([WOOFER_CONFIG.LEG_LR, WOOFER_CONFIG.LEG_FB, 0]) +foot_locs[3:6] = np.array([-WOOFER_CONFIG.LEG_LR, WOOFER_CONFIG.LEG_FB, 0]) +foot_locs[6:9] = np.array([WOOFER_CONFIG.LEG_LR, -WOOFER_CONFIG.LEG_FB, 0]) +foot_locs[9:12] = np.array([-WOOFER_CONFIG.LEG_LR, -WOOFER_CONFIG.LEG_FB, 0]) + + +u = mpc.solveSystem(reference_trajectory, foot_locs, 0.0) + +print(u[0:12]) diff --git a/woofer_numpy_log.npz b/woofer_numpy_log.npz index a1a7742..5d1f1c8 100644 Binary files a/woofer_numpy_log.npz and b/woofer_numpy_log.npz differ diff --git a/woofer_test.py b/woofer_test.py deleted file mode 100644 index 87f1307..0000000 --- a/woofer_test.py +++ /dev/null @@ -1,21 +0,0 @@ -from mujoco_py import load_model_from_path, MjSim, MjViewer -import numpy as np -np.set_printoptions(suppress=True) - -import woofer_xml_parser - -model = load_model_from_path("woofer_out.xml") -print(model) -sim = MjSim(model) -viewer = MjViewer(sim) - -sim.step() - -print("Nicely exposed function:\n") -print(sim.model.get_xml()) -# viewer.render() - - - - -