terminalCostModel), 0.)
terminalModel.differential.armature = np.matrix(
[0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.]).T
# For this optimal control problem, we define 250 knots (or running action
# models) plus a terminal knot
T = 250
q0 = np.matrix([0.173046, 1., -0.52366, 0., 0., 0.1, -0.005]).T
x0 = np.concatenate([q0, pinocchio.utils.zero(robot_model.nv)])
problem = crocoddyl.ShootingProblem(x0, [runningModel] * T, terminalModel)
# Creating the DDP solver for this OC problem, defining a logger
ddp = crocoddyl.SolverDDP(problem)
cameraTF = [2., 2.68, 0.54, 0.2, 0.62, 0.72, 0.22]
if WITHDISPLAY and WITHPLOT:
display = crocoddyl.GepettoDisplay(talos_arm, 4, 4, cameraTF)
ddp.setCallbacks([
crocoddyl.CallbackLogger(),
crocoddyl.CallbackVerbose(),
crocoddyl.CallbackDisplay(display)
])
elif WITHDISPLAY:
display = crocoddyl.GepettoDisplay(talos_arm, 4, 4, cameraTF)
ddp.setCallbacks(
[crocoddyl.CallbackVerbose(),
crocoddyl.CallbackDisplay(display)])
elif WITHPLOT:
ddp.setCallbacks([
crocoddyl.CallbackLogger(),
crocoddyl.CallbackVerbose(),
])
dt = 3e-2
runningModel = crocoddyl.IntegratedActionModelEuler(
crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actuation, runningCostModel), dt)
terminalModel = crocoddyl.IntegratedActionModelEuler(
crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actuation, terminalCostModel), dt)
# Creating the shooting problem and the FDDP solver
T = 33
problem = crocoddyl.ShootingProblem(np.concatenate([hector.q0, np.zeros(state.nv)]), [runningModel] * T, terminalModel)
solver = crocoddyl.SolverFDDP(problem)
solver.setCallbacks([crocoddyl.CallbackLogger(), crocoddyl.CallbackVerbose()])
cameraTF = [-0.03, 4.4, 2.3, -0.02, 0.56, 0.83, -0.03]
if WITHDISPLAY and WITHPLOT:
display = crocoddyl.GepettoDisplay(hector, 4, 4, cameraTF)
solver.setCallbacks([crocoddyl.CallbackLogger(), crocoddyl.CallbackVerbose(), crocoddyl.CallbackDisplay(display)])
elif WITHDISPLAY:
display = crocoddyl.GepettoDisplay(hector, 4, 4, cameraTF)
solver.setCallbacks([crocoddyl.CallbackVerbose(), crocoddyl.CallbackDisplay(display)])
elif WITHPLOT:
solver.setCallbacks([crocoddyl.CallbackLogger(), crocoddyl.CallbackVerbose()])
else:
solver.setCallbacks([crocoddyl.CallbackVerbose()])
# Solving the problem with the FDDP solver
solver.solve()
# Plotting the entire motion
if WITHPLOT:
log = solver.getCallbacks()[0]
cameraTF = [3., 3.68, 0.84, 0.2, 0.62, 0.72, 0.22]
ddp = [None] * len(GAITPHASES)
for i, phase in enumerate(GAITPHASES):
for key, value in phase.items():
if key == 'walking':
# Creating a walking problem
ddp[i] = crocoddyl.SolverDDP(
gait.createWalkingProblem(x0, value['stepLength'], value['stepHeight'], value['timeStep'],
value['stepKnots'], value['supportKnots']))
ddp[i].th_stop = 1e-7
# Added the callback functions
print('*** SOLVE ' + key + ' ***')
if WITHDISPLAY and WITHPLOT:
display = crocoddyl.GepettoDisplay(talos_legs, 4, 4, cameraTF, frameNames=[rightFoot, leftFoot])
ddp[i].setCallbacks(
[crocoddyl.CallbackLogger(),
crocoddyl.CallbackVerbose(),
crocoddyl.CallbackDisplay(display)])
elif WITHDISPLAY:
display = crocoddyl.GepettoDisplay(talos_legs, 4, 4, cameraTF, frameNames=[rightFoot, leftFoot])
ddp[i].setCallbacks([crocoddyl.CallbackVerbose(), crocoddyl.CallbackDisplay(display)])
elif WITHPLOT:
ddp[i].setCallbacks([
crocoddyl.CallbackLogger(),
crocoddyl.CallbackVerbose(),
])
else:
ddp[i].setCallbacks([crocoddyl.CallbackVerbose()])
boxfddp = crocoddyl.SolverBoxFDDP(
gait.createJumpingProblem(x0, jumping_gait['jumpHeight'],
jumping_gait['jumpLength'],
jumping_gait['timeStep'],
jumping_gait['groundKnots'],
jumping_gait['flyingKnots']))
boxddp = crocoddyl.SolverBoxDDP(
gait.createJumpingProblem(x0, jumping_gait['jumpHeight'],
jumping_gait['jumpLength'],
jumping_gait['timeStep'],
jumping_gait['groundKnots'],
jumping_gait['flyingKnots']))
# Added the callback functions
if WITHDISPLAY and WITHPLOT:
display = crocoddyl.GepettoDisplay(
anymal, 4, 4, cameraTF, frameNames=[lfFoot, rfFoot, lhFoot, rhFoot])
boxfddp.setCallbacks([
crocoddyl.CallbackLogger(),
crocoddyl.CallbackVerbose(),
crocoddyl.CallbackDisplay(display)
])
boxddp.setCallbacks([
crocoddyl.CallbackLogger(),
crocoddyl.CallbackVerbose(),
crocoddyl.CallbackDisplay(display)
])
elif WITHDISPLAY:
display = crocoddyl.GepettoDisplay(
anymal, 4, 4, cameraTF, frameNames=[lfFoot, rfFoot, lhFoot, rhFoot])
boxfddp.setCallbacks(
[crocoddyl.CallbackVerbose(),
endEffectorId = rmodel.getFrameId(endEffector)
rightFootId = rmodel.getFrameId(rightFoot)
leftFootId = rmodel.getFrameId(leftFoot)
q0 = rmodel.referenceConfigurations["half_sitting"]
x0 = np.concatenate([q0, np.zeros(rmodel.nv)])
pinocchio.forwardKinematics(rmodel, rdata, q0)
pinocchio.updateFramePlacements(rmodel, rdata)
rfPos0 = rdata.oMf[rightFootId].translation
lfPos0 = rdata.oMf[leftFootId].translation
refGripper = rdata.oMf[rmodel.getFrameId("gripper_left_joint")].translation
comRef = (rfPos0 + lfPos0) / 2
comRef[2] = pinocchio.centerOfMass(rmodel, rdata, q0)[2].item()
# Initialize Gepetto viewer
if WITHDISPLAY:
display = crocoddyl.GepettoDisplay(robot, frameNames=[rightFoot, leftFoot])
display.robot.viewer.gui.addSphere(
'world/point', .05, [1., 0., 0., 1.]) # radius = .1, RGBA=1001
display.robot.viewer.gui.applyConfiguration(
'world/point',
target.tolist() + [0., 0., 0., 1.]) # xyz+quaternion
# Add contact to the model
contactModel = crocoddyl.ContactModelMultiple(state, actuation.nu)
supportContactModelLeft = crocoddyl.ContactModel6D(state, leftFootId,
pinocchio.SE3.Identity(),
actuation.nu,
np.array([0, 0]))
contactModel.addContact(leftFoot + "_contact", supportContactModelLeft)
supportContactModelRight = crocoddyl.ContactModel6D(state, rightFootId,
pinocchio.SE3.Identity(),
runningModel = crocoddyl.IntegratedActionModelEuler(
crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actModel,
runningCostModel), dt)
terminalModel = crocoddyl.IntegratedActionModelEuler(
crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actModel,
terminalCostModel), dt)
# Creating the shooting problem and the FDDP solver
T = 100
x0 = np.array([3.14, 0, 0., 0.])
problem = crocoddyl.ShootingProblem(x0, [runningModel] * T, terminalModel)
fddp = crocoddyl.SolverFDDP(problem)
cameraTF = [1.4, 0., 0.2, 0.5, 0.5, 0.5, 0.5]
if WITHDISPLAY and WITHPLOT:
display = crocoddyl.GepettoDisplay(robot, 4, 4, cameraTF, False)
fddp.setCallbacks([
crocoddyl.CallbackLogger(),
crocoddyl.CallbackVerbose(),
crocoddyl.CallbackDisplay(display)
])
elif WITHDISPLAY:
display = crocoddyl.GepettoDisplay(robot, 4, 4, cameraTF, False)
fddp.setCallbacks(
[crocoddyl.CallbackVerbose(),
crocoddyl.CallbackDisplay(display)])
elif WITHPLOT:
fddp.setCallbacks(
[crocoddyl.CallbackLogger(),
crocoddyl.CallbackVerbose()])
else:
qnew = [*q0[0:7], *[0.0, 0.0, 0.0]]
# calculate angle of the wheel
qw1 = np.arctan2(q0[8], q0[9])
qw2 = np.arctan2(q0[10], q0[11])
x0 = np.concatenate([q0, pinocchio.utils.zero(state.nv)])
problem = crocoddyl.ShootingProblem(x0, [runningModel] * T, terminalModel)
# Creating the DDP solver for this OC problem, defining a logger
ddp = crocoddyl.SolverFDDP(problem)
ddp.setCallbacks([crocoddyl.CallbackLogger(), crocoddyl.CallbackVerbose()])
cameraTF = [3., 2.68, 1.54, 0.2, 0.62, 0.72, 0.22]
if WITHDISPLAY and WITHPLOT:
display = crocoddyl.GepettoDisplay(robot, 4, 4, cameraTF)
ddp.setCallbacks([
crocoddyl.CallbackLogger(),
crocoddyl.CallbackVerbose(),
crocoddyl.CallbackDisplay(display)
])
elif WITHDISPLAY:
display = crocoddyl.GepettoDisplay(robot, 4, 4, cameraTF)
ddp.setCallbacks(
[crocoddyl.CallbackVerbose(),
crocoddyl.CallbackDisplay(display)])
elif WITHPLOT:
ddp.setCallbacks([
crocoddyl.CallbackLogger(),
crocoddyl.CallbackVerbose(),
])
import crocoddyl
import pinocchio
import numpy as np
import example_robot_data
robot = example_robot_data.load('talos_arm')
robot_model = robot.model
DT = 1e-3
T = 250
target = np.array([0.4, 0., .4])
cameraTF = [2., 2.68, 0.54, 0.2, 0.62, 0.72, 0.22]
display = crocoddyl.GepettoDisplay(robot, cameraTF=cameraTF, floor=False)
robot.viewer.gui.addSphere('world/point', .05,
[1., 0., 0., 1.]) # radius = .1, RGBA=1001
robot.viewer.gui.applyConfiguration('world/point',
target.tolist() +
[0., 0., 0., 1.]) # xyz+quaternion
robot.viewer.gui.refresh()
# Create the cost functions
Mref = crocoddyl.FrameTranslation(robot_model.getFrameId("gripper_left_joint"),
np.matrix(target).T)
state = crocoddyl.StateMultibody(robot.model)
goalTrackingCost = crocoddyl.CostModelFrameTranslation(state, Mref)
xRegCost = crocoddyl.CostModelState(state)
uRegCost = crocoddyl.CostModelControl(state)
# Create cost model per each action model
runningCostModel = crocoddyl.CostModelSum(state)
def generateTrajectory(self, **kwargs):
import time
import crocoddyl
from crocoddyl.utils.quadruped import SimpleQuadrupedalGaitProblem, plotSolution
# Load parameters of the trajectory
self.setParametersFromDict(**kwargs)
# Loading the solo model
solo = loadSolo(False)
robot_model = solo.model
# Set limits of actuators speeds and efforts
robot_model.effortLimit[6:] = np.full(12,
self.getParameter('torque_lim'))
robot_model.velocityLimit[6:] = np.tile(
np.deg2rad(self.getParameter('speed_lim')), 4)
# Setting up CoM problem
lfFoot, rfFoot, lhFoot, rhFoot = 'FL_FOOT', 'FR_FOOT', 'HL_FOOT', 'HR_FOOT'
gait = SimpleQuadrupedalGaitProblem(robot_model, lfFoot, rfFoot,
lhFoot, rhFoot)
# Defining the initial state of the robot
q0 = robot_model.referenceConfigurations['standing'].copy()
v0 = pin.utils.zero(robot_model.nv)
x0 = np.concatenate([q0, v0])
# Defining the CoM gait parameters
Jumping_gait = {}
Jumping_gait['jumpHeight'] = self.getParameter('height')
Jumping_gait['jumpLength'] = [
self.getParameter('dx'),
self.getParameter('dy'),
self.getParameter('dz')
]
Jumping_gait['timeStep'] = self.getParameter('dt')
Jumping_gait['groundKnots'] = self.getParameter('groundKnots')
Jumping_gait['flyingKnots'] = self.getParameter('flyingKnots')
# Setting up the control-limited DDP solver
boxddp = crocoddyl.SolverBoxDDP(
gait.createJumpingProblem(x0, Jumping_gait['jumpHeight'],
Jumping_gait['jumpLength'],
Jumping_gait['timeStep'],
Jumping_gait['groundKnots'],
Jumping_gait['flyingKnots']))
# Print debug info if requiered
t0 = time.time()
if self.getParameter('debug'):
print("Computing Trajectory...")
self.printInfo()
# Add the callback functions if requiered
if self.getParameter('verbose'):
boxddp.setCallbacks([crocoddyl.CallbackVerbose()])
# Setup viewer if requiered
cameraTF = [2., 2.68, 0.84, 0.2, 0.62, 0.72, 0.22]
if self.getParameter('gepetto_viewer'):
display = crocoddyl.GepettoDisplay(
solo,
4,
4,
cameraTF,
frameNames=[lfFoot, rfFoot, lhFoot, rhFoot])
boxddp.setCallbacks([
crocoddyl.CallbackVerbose(),
crocoddyl.CallbackDisplay(display)
])
xs = [robot_model.defaultState] * (boxddp.problem.T + 1)
us = boxddp.problem.quasiStatic([solo.model.defaultState] *
boxddp.problem.T)
# Solve the DDP problem
boxddp.solve(xs, us, self.getParameter('nb_it'), False, 0.1)
# Display the entire motion
if self.getParameter('gepetto_viewer'):
display = crocoddyl.GepettoDisplay(
solo, frameNames=[lfFoot, rfFoot, lhFoot, rhFoot])
while True:
display.displayFromSolver(boxddp)
# Get results from solver
xs, us = boxddp.xs, boxddp.us
nx, nq, nu = xs[0].shape[0], robot_model.nq, us[0].shape[0]
X = [0.] * nx
U = [0.] * nu
for i in range(nx):
X[i] = [np.asscalar(x[i]) for x in xs]
for i in range(nu):
U[i] = [np.asscalar(u[i]) if u.shape[0] != 0 else 0 for u in us]
qa = np.array(X[7:nq])[:, :-1]
qa_dot = np.array(X[nq + 6:2 * nq - 1])[:, :-1]
torques = np.array(U[:])
t_traj = np.arange(0, qa.shape[1]) * self.getParameter('dt')
# Print execution time if requiered
if self.getParameter('debug'):
print("Done in", time.time() - t0, "s")
# Define trajectory for return
traj = ActuatorsTrajectory()
traj.addElement('t', t_traj)
traj.addElement('q', qa)
traj.addElement('q_dot', qa_dot)
traj.addElement('torques', torques)
return traj
runningModel = crocoddyl.IntegratedActionModelEuler(
crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actuation, runningCostModel), dt)
terminalModel = crocoddyl.IntegratedActionModelEuler(
crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actuation, terminalCostModel), dt)
# Creating the shooting problem and the FDDP solver
T = 100
x0 = np.array([3.14, 0., 0., 0.])
problem = crocoddyl.ShootingProblem(x0, [runningModel] * T, terminalModel)
problem.nthreads = 1 # TODO(cmastalli): Remove after Crocoddyl supports multithreading with Python-derived models
solver = crocoddyl.SolverFDDP(problem)
cameraTF = [1.4, 0., 0.2, 0.5, 0.5, 0.5, 0.5]
if WITHDISPLAY and WITHPLOT:
display = crocoddyl.GepettoDisplay(pendulum, 4, 4, cameraTF, False)
solver.setCallbacks([crocoddyl.CallbackLogger(), crocoddyl.CallbackVerbose(), crocoddyl.CallbackDisplay(display)])
elif WITHDISPLAY:
display = crocoddyl.GepettoDisplay(pendulum, 4, 4, cameraTF, False)
solver.setCallbacks([crocoddyl.CallbackVerbose(), crocoddyl.CallbackDisplay(display)])
elif WITHPLOT:
solver.setCallbacks([crocoddyl.CallbackLogger(), crocoddyl.CallbackVerbose()])
else:
solver.setCallbacks([crocoddyl.CallbackVerbose()])
# Solving the problem with the FDDP solver
solver.solve()
# Plotting the entire motion
if WITHPLOT:
log = solver.getCallbacks()[0]
import example_robot_data
import pinocchio
import lcmaes
WITHDISPLAY = 'display' in sys.argv or 'CROCODDYL_DISPLAY' in os.environ
WITHPLOT = 'plot' in sys.argv or 'CROCODDYL_PLOT' in os.environ
# WITHDISPLAY = 'display'# in sys.argv or 'CROCODDYL_DISPLAY' in os.environ
WITHPLOT = 'EMPTY'
crocoddyl.switchToNumpyMatrix()
# Loading the anymal model
anymal = example_robot_data.loadANYmal()
display = crocoddyl.GepettoDisplay(anymal, 4, 4)
# Defining the initial state of the robot
print('model name: ' + anymal.model.name)
data = anymal.model.createData()
q = anymal.model.referenceConfigurations['standing'].copy()
pinocchio.forwardKinematics(anymal.model, data, q)
display.robot.display(q)
#---------------------------------------------------
# display setup
frameCoMTrajNames = []
frameCoMTrajNames.append(str(anymal.model.getFrameId('root_joint')))
def run():
# Loading the anymal model
anymal = example_robot_data.load('anymal')
# nq is the dimension fo the configuration vector representation
# nv dimension of the velocity vector space
# Defining the initial state of the robot
q0 = anymal.model.referenceConfigurations['standing'].copy()
v0 = pinocchio.utils.zero(anymal.model.nv)
x0 = np.concatenate([q0, v0])
# Setting up the 3d walking problem
lfFoot, rfFoot, lhFoot, rhFoot = 'LF_FOOT', 'RF_FOOT', 'LH_FOOT', 'RH_FOOT'
gait = SimpleQuadrupedalGaitProblem(anymal.model, lfFoot, rfFoot, lhFoot, rhFoot)
cameraTF = [2., 2.68, 0.84, 0.2, 0.62, 0.72, 0.22]
walking = {
'stepLength': 0.25,
'stepHeight': 0.15,
'timeStep': 1e-2,
'stepKnots': 100,
'supportKnots': 2
}
# Creating a walking problem
ddp = crocoddyl.SolverFDDP(
gait.createWalkingProblem(x0, walking['stepLength'], walking['stepHeight'], walking['timeStep'],
walking['stepKnots'], walking['supportKnots']))
plot = False
display = False
if display:
# Added the callback functions
display = crocoddyl.GepettoDisplay(anymal, 4, 4, cameraTF, frameNames=[lfFoot, rfFoot, lhFoot, rhFoot])
ddp.setCallbacks(
[crocoddyl.CallbackLogger(),
crocoddyl.CallbackVerbose(),
crocoddyl.CallbackDisplay(display)])
# Solving the problem with the DDP solver
xs = [anymal.model.defaultState] * (ddp.problem.T + 1)
us = ddp.problem.quasiStatic([anymal.model.defaultState] * ddp.problem.T)
ddp.solve(xs, us, 100, False, 0.1)
if display:
# Defining the final state as initial one for the next phase
# Display the entire motion
display = crocoddyl.GepettoDisplay(anymal, frameNames=[lfFoot, rfFoot, lhFoot, rhFoot])
display.displayFromSolver(ddp)
# Plotting the entire motion
if plot:
plotSolution(ddp, figIndex=1, show=False)
log = ddp.getCallbacks()[0]
crocoddyl.plotConvergence(log.costs,
log.u_regs,
log.x_regs,
log.grads,
log.stops,
log.steps,
figTitle='walking',
figIndex=3,
show=True)