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)
