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