def __init__(self, neural_net, use_gauss_approx=False): """ The neural network is a residual network: x ---> R**2, where R is the residual matrix. @params: 1: neural_net = must be the residual network 2: use_gauss_approx = to enable gauss approximation of the gradient and hessian A. The gradient(& hessian) needed for data.Lx can be calculated in two ways: 1: Calculate the Jacobian of the output of the neural network w.r.t input to get Lx 1.1: Calculate the Hessian of the output of the neural network w.r.t input to get Lxx B. Using Gauss approximation, the gradient and hessians are calculated like so: 1: Gradient = data.Lx = J.T @ r 1.1 Hessian = data.Lxx = J.T @ J, where J is the jacobian of the residual matrix and r is the residual matrix if A is used, then the activation of the residual network must be tanh() """ crocoddyl.ActionModelAbstract.__init__(self, crocoddyl.StateVector(3), 2, 5) self.net = neural_net self.use_gauss = use_gauss_approx
def __init__(self): crocoddyl.DifferentialActionModelAbstract.__init__( self, crocoddyl.StateVector(4), 1, 6) # nu = 1; nr = 6 self.unone = np.zeros(self.nu) self.m1 = 1. self.m2 = .1 self.l = .5 self.g = 9.81 self.costWeights = [1., 1., 0.1, 0.001, 0.001, 1.] # sin, 1-cos, x, xdot, thdot, f
def __init__(self, nx, nu, driftFree=True): crocoddyl.ActionModelAbstract.__init__(self, crocoddyl.StateVector(nx), nu) self.Fx = np.matrix(np.eye(self.state.nx)) self.Fu = np.matrix(np.eye(self.state.nx))[:, :self.nu] self.f0 = np.matrix(np.zeros(self.state.nx)).T self.Lxx = np.matrix(np.eye(self.state.nx)) self.Lxu = np.matrix(np.eye(self.state.nx))[:, :self.nu] self.Luu = np.matrix(np.eye(self.nu)) self.lx = np.matrix(np.ones(self.state.nx)).T self.lu = np.matrix(np.ones(self.nu)).T
def __init__(self, net): """ @params 1: network usage: terminal_model = UnicycleTerminal(net) """ crocoddyl.ActionModelAbstract.__init__(self, crocoddyl.StateVector(3), 2, 5) self.net = net
def __init__(self, nq, nu, driftFree=True): crocoddyl.DifferentialActionModelAbstract.__init__(self, crocoddyl.StateVector(2 * nq), nu) self.Fq = np.matrix(np.eye(self.state.nq)) self.Fv = np.matrix(np.eye(self.state.nv)) self.Fu = np.matrix(np.eye(self.state.nq))[:, :self.nu] self.f0 = np.matrix(np.zeros(self.state.nv)).T self.Lxx = np.matrix(np.eye(self.state.nx)) self.Lxu = np.matrix(np.eye(self.state.nx))[:, :self.nu] self.Luu = np.matrix(np.eye(self.nu)) self.lx = np.matrix(np.ones(self.state.nx)).T self.lu = np.matrix(np.ones(self.nu)).T
def __init__(self, target): crocoddyl.DifferentialActionModelAbstract.__init__( self, crocoddyl.StateVector(4), 2, 3) # nu = 1; nr = 6 a = 0.2 b = 0.2 c = 0.3 d = 0.4 e = 0.4 self.params = (a, b, c, d, e) self.costWeights = [1e1, .01, 1e2] # sin, 1-cos, x, xdot, thdot, f self.time_elapsed = 0 self.target = target
def __init__(self, diffModel, timeStep=1e-3, withCostResiduals=True, alpha=0): crocoddyl.ActionModelAbstract.__init__( self, crocoddyl.StateVector(diffModel.state.nx + diffModel.nu), diffModel.nu) self.differential = diffModel self.timeStep = timeStep self.withCostResiduals = withCostResiduals self.alpha = alpha self.nx = diffModel.state.nx self.nw = diffModel.nu # augmented dimension self.ny = self.nu + self.nx # print('INIT : nx ', self.nx, ' ny : ', self.ny, ' nw : ', self.nw) if self.timeStep == 0: self.enable_integration_ = False else: self.enable_integration_ = True
def __init__(self, neural_net): crocoddyl.ActionModelAbstract.__init__(self, crocoddyl.StateVector(3), 2, 5) self.net = neural_net
def __init__(self): crocoddyl.ActionModelAbstract.__init__(self, crocoddyl.StateVector(3), 2, 5) self.dt = .1 self.costWeights = [10., 1.]
def __init__(self, nx=12, nu=12, mu=0.8, log=True): crocoddyl.ActionModelAbstract.__init__(self, crocoddyl.StateVector(nx), nu, 1) #:param state: state description, #:param nu: dimension of control vector, #:param nr: dimension of the cost-residual vector (default 1) # Time step of the solver self.dt = 0.02 # Mass of the robot self.mass = 2.97784899 # Inertia matrix of the robot in body frame (found in urdf) self.gI = np.diag([0.00578574, 0.01938108, 0.02476124]) # Friction coefficient self.mu = mu # Matrix A -- X_k+1 = AX + BU -- self.A = np.zeros((12, 12)) # Matrix B self.B = np.zeros((12, 12)) # Vector xref self.xref = np.zeros(12) # Vector g self.g = np.zeros((12, )) # Weight vector for the state self.stateWeight = np.full(12, 2) # Weight on the friction cost self.frictionWeight = 0.1 # Weight on the command forces self.weightForces = np.full(12, 0.1) # Levers Arms used in B self.lever_arms = np.zeros((3, 4)) # Initial position of footholds in the "straight standing" default configuration # footholds = [[ px_foot1 , px_foot2 ... ] , # [ py_foot1 , py_foot2 ... ] , # [ pz_foot1 , pz_foot2 ... ] ] 2D -- pz = 0 self.footholds = np.array([[0.19, 0.19, -0.19, -0.19], [0.15005, -0.15005, 0.15005, -0.15005], [0.0, 0.0, 0.0, 0.0]]) # S matrix # Represents the feet that are in contact with the ground such as : # S = [1 0 0 1] --> FL(Front Left) in contact, FR not , HL not , HR in contact self.S = np.ones(4) # List of the feet in contact with the ground used to compute B # if S = [1 0 0 1] ; L_feet = [1 1 1 0 0 0 0 0 0 1 1 1] self.L_feet = np.zeros(12) # Cone crocoddyl class R = np.identity(3) # Rotation matrix wrt inertial frame number_facets = 4 # Number of facets for cone approximation self.cone = crocoddyl.FrictionCone(R, self.mu, number_facets, False) self.lb = np.tile(self.cone.lb, 4) self.ub = np.tile(self.cone.ub, 4) # Matrice Fa of the cone class, repeated 4 times (4 feet) self.Fa = np.zeros((20, 12)) # Inequality activation model. # The activation is zero when r is between the lower (lb) and upper (ub) bounds, beta # determines how much of the total range is not activated. This is the activation # equations: # a(r) = 0.5 * ||r||^2 for lb < r < ub # a(r) = 0. for lb >= r >= ub. self.activation = crocoddyl.ActivationModelQuadraticBarrier( (crocoddyl.ActivationBounds(self.lb, self.ub))) self.dataCost = self.activation.createData() self.Lu_f = np.zeros(12) self.Luu_f = np.zeros((12, 12)) # Log parameters self.log = log self.log_cost_friction = [] self.log_cost_state = [] self.log_state = [] self.log_u = [] # Other dynamic models self.I_inv = np.zeros((3, 3)) self.derivative_B = np.zeros( (12, 12)) # Here lever arm is constant, not used
class StateVectorTest(StateAbstractTestCase): NX = randint(1, 101) NDX = StateAbstractTestCase.NX STATE = crocoddyl.StateVector(NX) STATE_DER = StateVectorDerived(NX)
def __init__(self, neural_net): crocoddyl.ActionModelAbstract.__init__(self, crocoddyl.StateVector(3), 2, 5) self.net = neural_net device = torch.device('cpu') self.net.to(device)
def __init__(self): crocoddyl.ActionModelAbstract.__init__(self, crocoddyl.StateVector(3), 2, 5)