Merging MPC controller

.gitignore

@@ -0,0 +1 @@
+.DS_Store

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)
+	return kp*(refpos-qpos)

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

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