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
gl.glColor3f(0.9, 0.2, 0.2)
gl.sphere(icp[0], icp[1], 0, 0.01, 20, 20)
coefs = np.array(msg.coefs)
ts = np.array(msg.ts)
if ts.shape[0] == 0:
gl.switch_buffer()
return
# draw spline segments
tsdense = np.linspace(ts[0], ts[-1], 30)
gl.glColor3f(1.0, 0.2, 0.2)
gl.glLineWidth(1)
gl.glBegin(GL_LINES)
ps = np.array([ppval(coefs, msg.ts, t) for t in tsdense])
for j in range(0, tsdense.size - 1):
gl.glVertex3f(ps[j, 0], ps[j, 1], ps[j, 2])
gl.glVertex3f(ps[j + 1, 0], ps[j + 1, 1], ps[j + 1, 2])
gl.glEnd()
gl.switch_buffer()
lc = lcm.LCM()
gl = lcmgl('reactive recovery debug', lc)
subscription = lc.subscribe("REACTIVE_RECOVERY_DEBUG", handle_debug_msg)
try:
while True:
lc.handle()
except KeyboardInterrupt:
pass
import lcm
import time
from bot_lcmgl import lcmgl
class Behavior:
WALKING = 0
CRAWLING = 1
BDI_WALKING = 2
BDI_STEPPING = 3
lc = lcm.LCM()
gl = lcmgl('BDI_Step_Translator', lc)
def now_utime():
return int(time.time() * 1000000)
import lcm
import time
from bot_lcmgl import lcmgl
class Behavior:
WALKING = 0
CRAWLING = 1
BDI_WALKING = 2
BDI_STEPPING = 3
lc = lcm.LCM()
gl = lcmgl('BDI_Step_Translator', lc)
def now_utime():
return int(time.time() * 1000000)
gl.glColor3f(0.9,0.2,0.2);
gl.sphere(icp[0], icp[1], 0, 0.01, 20, 20);
coefs = np.array(msg.coefs)
ts = np.array(msg.ts)
if ts.shape[0] == 0:
gl.switch_buffer()
return
# draw spline segments
tsdense = np.linspace(ts[0], ts[-1], 30)
gl.glColor3f(1.0, 0.2, 0.2);
gl.glLineWidth(1);
gl.glBegin(GL_LINES);
ps = np.array([ppval(coefs, msg.ts, t) for t in tsdense]);
for j in range(0,tsdense.size-1):
gl.glVertex3f(ps[j,0], ps[j,1], ps[j,2]);
gl.glVertex3f(ps[j+1,0], ps[j+1,1], ps[j+1,2]);
gl.glEnd();
gl.switch_buffer()
lc = lcm.LCM()
gl = lcmgl('reactive recovery debug', lc);
subscription = lc.subscribe("REACTIVE_RECOVERY_DEBUG", handle_debug_msg)
try:
while True:
lc.handle()
except KeyboardInterrupt:
pass
''' Listens to QP Controller Inputs and draws, in different but
order-consistent colors, the cubic splines being followed by each
body motion block. '''
import lcm
import drc
from drake import lcmt_qp_controller_input, lcmt_body_motion_data
from drc import robot_state_t
import sys
import time
from bot_lcmgl import lcmgl, GL_LINES
import numpy as np
import math
lc = lcm.LCM()
gl = lcmgl('qp input bmd snoop', lc);
color_order = [[1.0, 0.1, 0.1], [0.1, 1.0, 0.1], [0.1, 0.1, 1.0], [1.0, 1.0, 0.1], [1.0, 0.1, 1.0], [0.1, 1.0, 1.0]];
t_global = 0;
def pval(coefs, t_off):
out = 0.;
for j in range(0, coefs.size):
out += coefs[j]*(t_off**j)
return out
def handle_robot_state_msg(channel, data):
global t_global
msg = robot_state_t.decode(data)
t_global = msg.utime/1E6;