def __init__(self,
rtree,
q_nom,
control_period=0.001,
print_period=1.0,
sim=True,
cost_pub=False):
LeafSystem.__init__(self)
self.rtree = rtree
self.nq = rtree.get_num_positions()
self.nv = rtree.get_num_velocities()
self.nu = rtree.get_num_actuators()
self.cost_pub = cost_pub
dim = 3 # 3D
nd = 4 # for friction cone approx, hard coded for now
self.nc = 4 # number of contacts; TODO don't hard code
self.nf = self.nc * nd # number of contact force variables
self.ncf = 2 # number of loop constraint forces; TODO don't hard code
self.neps = self.nc * dim # number of slack variables for contact
if sim:
self.sim = True
self._DeclareInputPort(PortDataType.kVectorValued,
self.nq + self.nv)
self._DeclareDiscreteState(self.nu)
self._DeclareVectorOutputPort(BasicVector(self.nu),
self.CopyStateOutSim)
self.print_period = print_period
self.last_print_time = -print_period
self.last_v = np.zeros(3)
self.lcmgl = lcmgl("Balancing-plan", true_lcm.LCM())
# self.lcmgl = None
else:
self.sim = False
self._DeclareInputPort(PortDataType.kVectorValued, 1)
self._DeclareInputPort(PortDataType.kVectorValued,
kCassiePositions)
self._DeclareInputPort(PortDataType.kVectorValued,
kCassieVelocities)
self._DeclareInputPort(PortDataType.kVectorValued, 2)
self._DeclareDiscreteState(kCassieActuators * 8 + 1)
self._DeclareVectorOutputPort(
BasicVector(1),
lambda c, o: self.CopyStateOut(c, o, 8 * kCassieActuators, 1))
self._DeclareVectorOutputPort(
BasicVector(kCassieActuators), lambda c, o: self.CopyStateOut(
c, o, 0, kCassieActuators)) #torque
self._DeclareVectorOutputPort(
BasicVector(kCassieActuators), lambda c, o: self.CopyStateOut(
c, o, kCassieActuators, kCassieActuators)) #motor_pos
self._DeclareVectorOutputPort(
BasicVector(kCassieActuators), lambda c, o: self.CopyStateOut(
c, o, 2 * kCassieActuators, kCassieActuators)) #motor_vel
self._DeclareVectorOutputPort(
BasicVector(kCassieActuators), lambda c, o: self.CopyStateOut(
c, o, 3 * kCassieActuators, kCassieActuators)) #kp
self._DeclareVectorOutputPort(
BasicVector(kCassieActuators), lambda c, o: self.CopyStateOut(
c, o, 4 * kCassieActuators, kCassieActuators)) #kd
self._DeclareVectorOutputPort(
BasicVector(kCassieActuators), lambda c, o: self.CopyStateOut(
c, o, 5 * kCassieActuators, kCassieActuators)) #ki
self._DeclareVectorOutputPort(
BasicVector(kCassieActuators), lambda c, o: self.CopyStateOut(
c, o, 6 * kCassieActuators, kCassieActuators)) #leak
self._DeclareVectorOutputPort(
BasicVector(kCassieActuators), lambda c, o: self.CopyStateOut(
c, o, 7 * kCassieActuators, kCassieActuators)) #clamp
self._DeclarePeriodicDiscreteUpdate(0.0005)
self.q_nom = q_nom
self.com_des = rtree.centerOfMass(self.rtree.doKinematics(q_nom))
self.u_des = CassieFixedPointTorque()
self.lfoot = rtree.FindBody("toe_left")
self.rfoot = rtree.FindBody("toe_right")
self.springs = 2 + np.array([
rtree.FindIndexOfChildBodyOfJoint("knee_spring_left_fixed"),
rtree.FindIndexOfChildBodyOfJoint("knee_spring_right_fixed"),
rtree.FindIndexOfChildBodyOfJoint("ankle_spring_joint_left"),
rtree.FindIndexOfChildBodyOfJoint("ankle_spring_joint_right")
])
umin = np.zeros(self.nu)
umax = np.zeros(self.nu)
ii = 0
for motor in rtree.actuators:
umin[ii] = motor.effort_limit_min
umax[ii] = motor.effort_limit_max
ii += 1
slack_limit = 10.0
self.initialized = False
#------------------------------------------------------------
# Add Decision Variables ------------------------------------
prog = MathematicalProgram()
qddot = prog.NewContinuousVariables(self.nq, "joint acceleration")
u = prog.NewContinuousVariables(self.nu, "input")
bar = prog.NewContinuousVariables(self.ncf, "four bar forces")
beta = prog.NewContinuousVariables(self.nf, "friction forces")
eps = prog.NewContinuousVariables(self.neps, "slack variable")
nparams = prog.num_vars()
#------------------------------------------------------------
# Problem Constraints ---------------------------------------
self.con_u_lim = prog.AddBoundingBoxConstraint(umin, umax,
u).evaluator()
self.con_fric_lim = prog.AddBoundingBoxConstraint(0, 100000,
beta).evaluator()
self.con_slack_lim = prog.AddBoundingBoxConstraint(
-slack_limit, slack_limit, eps).evaluator()
bar_con = np.zeros((self.ncf, self.nq))
self.con_4bar = prog.AddLinearEqualityConstraint(
bar_con, np.zeros(self.ncf), qddot).evaluator()
if self.nc > 0:
dyn_con = np.zeros(
(self.nq, self.nq + self.nu + self.ncf + self.nf))
dyn_vars = np.concatenate((qddot, u, bar, beta))
self.con_dyn = prog.AddLinearEqualityConstraint(
dyn_con, np.zeros(self.nq), dyn_vars).evaluator()
foot_con = np.zeros((self.neps, self.nq + self.neps))
foot_vars = np.concatenate((qddot, eps))
self.con_foot = prog.AddLinearEqualityConstraint(
foot_con, np.zeros(self.neps), foot_vars).evaluator()
else:
dyn_con = np.zeros((self.nq, self.nq + self.nu + self.ncf))
dyn_vars = np.concatenate((qddot, u, bar))
self.con_dyn = prog.AddLinearEqualityConstraint(
dyn_con, np.zeros(self.nq), dyn_vars).evaluator()
#------------------------------------------------------------
# Problem Costs ---------------------------------------------
self.Kp_com = 50
self.Kd_com = 2.0 * sqrt(self.Kp_com)
# self.Kp_qdd = 10*np.eye(self.nq)#np.diag(np.diag(H)/np.max(np.diag(H)))
self.Kp_qdd = np.diag(
np.concatenate(([10, 10, 10, 5, 5, 5], np.zeros(self.nq - 6))))
self.Kd_qdd = 1.0 * np.sqrt(self.Kp_qdd)
self.Kp_qdd[self.springs, self.springs] = 0
self.Kd_qdd[self.springs, self.springs] = 0
com_ddot_des = np.zeros(dim)
qddot_des = np.zeros(self.nq)
self.w_com = 0
self.w_qdd = np.zeros(self.nq)
self.w_qdd[:6] = 10
self.w_qdd[8:10] = 1
self.w_qdd[3:6] = 1000
# self.w_qdd[self.springs] = 0
self.w_u = 0.001 * np.ones(self.nu)
self.w_u[2:4] = 0.01
self.w_slack = 0.1
self.cost_qdd = prog.AddQuadraticErrorCost(np.diag(self.w_qdd),
qddot_des,
qddot).evaluator()
self.cost_u = prog.AddQuadraticErrorCost(np.diag(self.w_u), self.u_des,
u).evaluator()
self.cost_slack = prog.AddQuadraticErrorCost(
self.w_slack * np.eye(self.neps), np.zeros(self.neps),
eps).evaluator()
# self.cost_com = prog.AddQuadraticCost().evaluator()
# self.cost_qdd = prog.AddQuadraticCost(
# 2*np.diag(self.w_qdd), -2*np.matmul(qddot_des, np.diag(self.w_qdd)), qddot).evaluator()
# self.cost_u = prog.AddQuadraticCost(
# 2*np.diag(self.w_u), -2*np.matmul(self.u_des, np.diag(self.w_u)) , u).evaluator()
# self.cost_slack = prog.AddQuadraticCost(
# 2*self.w_slack*np.eye(self.neps), np.zeros(self.neps), eps).evaluator()
REG = 1e-8
self.cost_reg = prog.AddQuadraticErrorCost(
REG * np.eye(nparams), np.zeros(nparams),
prog.decision_variables()).evaluator()
# self.cost_reg = prog.AddQuadraticCost(2*REG*np.eye(nparams),
# np.zeros(nparams), prog.decision_variables()).evaluator()
#------------------------------------------------------------
# Solver settings -------------------------------------------
self.solver = GurobiSolver()
# self.solver = OsqpSolver()
prog.SetSolverOption(SolverType.kGurobi, "Method", 2)
#------------------------------------------------------------
# Save MathProg Variables -----------------------------------
self.qddot = qddot
self.u = u
self.bar = bar
self.beta = beta
self.eps = eps
self.prog = prog
def __init__(self, rtree, q_nom, control_period=0.001):
self.rtree = rtree
self.nq = rtree.get_num_positions()
self.nv = rtree.get_num_velocities()
self.nu = rtree.get_num_actuators()
dim = 3 # 3D
nd = 4 # for friction cone approx, hard coded for now
self.nc = 4 # number of contacts; TODO don't hard code
self.nf = self.nc * nd # number of contact force variables
self.ncf = 2 # number of loop constraint forces; TODO don't hard code
self.neps = self.nc * dim # number of slack variables for contact
self.q_nom = q_nom
self.com_des = rtree.centerOfMass(self.rtree.doKinematics(q_nom))
self.u_des = CassieFixedPointTorque()
self.lfoot = rtree.FindBody("toe_left")
self.rfoot = rtree.FindBody("toe_right")
self.springs = 2 + np.array([
rtree.FindIndexOfChildBodyOfJoint("knee_spring_left_fixed"),
rtree.FindIndexOfChildBodyOfJoint("knee_spring_right_fixed"),
rtree.FindIndexOfChildBodyOfJoint("ankle_spring_joint_left"),
rtree.FindIndexOfChildBodyOfJoint("ankle_spring_joint_right")
])
umin = np.zeros(self.nu)
umax = np.zeros(self.nu)
ii = 0
for motor in rtree.actuators:
umin[ii] = motor.effort_limit_min
umax[ii] = motor.effort_limit_max
ii += 1
slack_limit = 10.0
self.initialized = False
#------------------------------------------------------------
# Add Decision Variables ------------------------------------
prog = MathematicalProgram()
qddot = prog.NewContinuousVariables(self.nq, "joint acceleration")
u = prog.NewContinuousVariables(self.nu, "input")
bar = prog.NewContinuousVariables(self.ncf, "four bar forces")
beta = prog.NewContinuousVariables(self.nf, "friction forces")
eps = prog.NewContinuousVariables(self.neps, "slack variable")
nparams = prog.num_vars()
#------------------------------------------------------------
# Problem Constraints ---------------------------------------
self.con_u_lim = prog.AddBoundingBoxConstraint(umin, umax,
u).evaluator()
self.con_fric_lim = prog.AddBoundingBoxConstraint(0, 100000,
beta).evaluator()
self.con_slack_lim = prog.AddBoundingBoxConstraint(
-slack_limit, slack_limit, eps).evaluator()
bar_con = np.zeros((self.ncf, self.nq))
self.con_4bar = prog.AddLinearEqualityConstraint(
bar_con, np.zeros(self.ncf), qddot).evaluator()
if self.nc > 0:
dyn_con = np.zeros(
(self.nq, self.nq + self.nu + self.ncf + self.nf))
dyn_vars = np.concatenate((qddot, u, bar, beta))
self.con_dyn = prog.AddLinearEqualityConstraint(
dyn_con, np.zeros(self.nq), dyn_vars).evaluator()
foot_con = np.zeros((self.neps, self.nq + self.neps))
foot_vars = np.concatenate((qddot, eps))
self.con_foot = prog.AddLinearEqualityConstraint(
foot_con, np.zeros(self.neps), foot_vars).evaluator()
else:
dyn_con = np.zeros((self.nq, self.nq + self.nu + self.ncf))
dyn_vars = np.concatenate((qddot, u, bar))
self.con_dyn = prog.AddLinearEqualityConstraint(
dyn_con, np.zeros(self.nq), dyn_vars).evaluator()
#------------------------------------------------------------
# Problem Costs ---------------------------------------------
self.Kp_com = 50
self.Kd_com = 2.0 * sqrt(self.Kp_com)
# self.Kp_qdd = 10*np.eye(self.nq)#np.diag(np.diag(H)/np.max(np.diag(H)))
self.Kp_qdd = np.diag(
np.concatenate(([10, 10, 10, 5, 5, 5], np.zeros(self.nq - 6))))
self.Kd_qdd = 1.0 * np.sqrt(self.Kp_qdd)
self.Kp_qdd[self.springs, self.springs] = 0
self.Kd_qdd[self.springs, self.springs] = 0
com_ddot_des = np.zeros(dim)
qddot_des = np.zeros(self.nq)
self.w_com = 0
self.w_qdd = np.zeros(self.nq)
self.w_qdd[:6] = 10
self.w_qdd[8:10] = 1
self.w_qdd[3:6] = 1000
# self.w_qdd[self.springs] = 0
self.w_u = 0.001 * np.ones(self.nu)
self.w_u[2:4] = 0.01
self.w_slack = 0.1
self.cost_qdd = prog.AddQuadraticErrorCost(np.diag(self.w_qdd),
qddot_des,
qddot).evaluator()
self.cost_u = prog.AddQuadraticErrorCost(np.diag(self.w_u), self.u_des,
u).evaluator()
self.cost_slack = prog.AddQuadraticErrorCost(
self.w_slack * np.eye(self.neps), np.zeros(self.neps),
eps).evaluator()
# self.cost_com = prog.AddQuadraticCost().evaluator()
# self.cost_qdd = prog.AddQuadraticCost(
# 2*np.diag(self.w_qdd), -2*np.matmul(qddot_des, np.diag(self.w_qdd)), qddot).evaluator()
# self.cost_u = prog.AddQuadraticCost(
# 2*np.diag(self.w_u), -2*np.matmul(self.u_des, np.diag(self.w_u)) , u).evaluator()
# self.cost_slack = prog.AddQuadraticCost(
# 2*self.w_slack*np.eye(self.neps), np.zeros(self.neps), eps).evaluator()
REG = 1e-8
self.cost_reg = prog.AddQuadraticErrorCost(
REG * np.eye(nparams), np.zeros(nparams),
prog.decision_variables()).evaluator()
# self.cost_reg = prog.AddQuadraticCost(2*REG*np.eye(nparams),
# np.zeros(nparams), prog.decision_variables()).evaluator()
#------------------------------------------------------------
# Solver settings -------------------------------------------
self.solver = GurobiSolver()
# self.solver = OsqpSolver()
prog.SetSolverOption(SolverType.kGurobi, "Method", 2)
#------------------------------------------------------------
# Save MathProg Variables -----------------------------------
self.qddot = qddot
self.u = u
self.bar = bar
self.beta = beta
self.eps = eps
self.prog = prog
def optimal_trajectory(joints,
start_position,
end_position,
signed_dist_fn,
nc,
time=10,
knot_points=100):
assert len(joints) == len(start_position)
assert len(joints) == len(end_position)
h = time / (knot_points - 1)
nq = len(joints)
prog = MathematicalProgram()
q_var = []
v_var = []
for ii in range(knot_points):
q_var.append(prog.NewContinuousVariables(nq, "q[" + str(ii) + "]"))
v_var.append(prog.NewContinuousVariables(nq, "v[" + str(ii) + "]"))
# ---------------------------------------------------------------
# Initial & Final Pose Constraints ------------------------------
x_i = np.append(start_position, np.zeros(nq))
x_i_vars = np.append(q_var[0], v_var[0])
prog.AddLinearEqualityConstraint(np.eye(2 * nq), x_i, x_i_vars)
tol = 0.01 * np.ones(2 * nq)
x_f = np.append(end_position, np.zeros(nq))
x_f_vars = np.append(q_var[-1], v_var[-1])
prog.AddBoundingBoxConstraint(x_f - tol, x_f + tol, x_f_vars)
# ---------------------------------------------------------------
# Dynamics Constraints ------------------------------------------
for ii in range(knot_points - 1):
dyn_con1 = np.hstack((np.eye(nq), np.eye(nq), -np.eye(nq)))
dyn_var1 = np.concatenate((q_var[ii], v_var[ii], q_var[ii + 1]))
prog.AddLinearEqualityConstraint(dyn_con1, np.zeros(nq), dyn_var1)
# ---------------------------------------------------------------
# Joint Limit Constraints ---------------------------------------
q_min = np.array([j.lower_limits() for j in joints])
q_max = np.array([j.upper_limits() for j in joints])
for ii in range(knot_points):
prog.AddBoundingBoxConstraint(q_min, q_max, q_var[ii])
# ---------------------------------------------------------------
# Collision Constraints -----------------------------------------
for ii in range(knot_points):
prog.AddConstraint(signed_dist_fn, np.zeros(nc), 1e8 * np.ones(nc),
q_var[ii])
# ---------------------------------------------------------------
# Dynamics Constraints ------------------------------------------
for ii in range(knot_points):
prog.AddQuadraticErrorCost(np.eye(nq), np.zeros(nq), v_var[ii])
xi = np.array(start_position)
xf = np.array(end_position)
for ii in range(knot_points):
prog.SetInitialGuess(q_var[ii],
ii * (xf - xi) / (knot_points - 1) + xi)
prog.SetInitialGuess(v_var[ii], np.zeros(nq))
result = prog.Solve()
print prog.GetSolverId().name()
if result != SolutionResult.kSolutionFound:
print result
return None
q_path = []
# print signed_dist_fn(prog.GetSolution(q_var[0]))
for ii in range(knot_points):
q_path.append(prog.GetSolution(q_var[ii]))
return q_path
