def loadSoloLeg(solo8=True):
if solo8:
URDF_FILENAME = "solo.urdf"
legMaxId = 4
else:
URDF_FILENAME = "solo12.urdf"
legMaxId = 5
SRDF_FILENAME = "solo.srdf"
SRDF_SUBPATH = "/solo_description/srdf/" + SRDF_FILENAME
URDF_SUBPATH = "/solo_description/robots/" + URDF_FILENAME
modelPath = example_robot_data.getModelPath(URDF_SUBPATH)
robot = example_robot_data.loadSolo(solo8)
m1 = robot.model
m2 = pinocchio.Model()
for index, [j, M, name, parent, Y] in enumerate(
zip(m1.joints, m1.jointPlacements, m1.names, m1.parents,
m1.inertias)):
if j.id < legMaxId:
jointType = j.shortname()
if jointType == 'JointModelFreeFlyer':
# for the freeflyer we just take reduced mass and zero inertia
jointType = 'JointModelPZ'
Y.mass = Y.mass / 4
Y.inertia = np.diag(1e-6 * np.ones(3))
M = pinocchio.SE3.Identity()
if index == 2:
# start with the prismatic joint on the sliding guide
M.translation = np.zeros(3)
# 2D model, flatten y axis
vector2d = M.translation
vector2d[1] = 0.0
M.translation = vector2d
jid = m2.addJoint(parent, getattr(pinocchio, jointType)(), M, name)
assert (jid == j.id)
m2.appendBodyToJoint(jid, Y, pinocchio.SE3.Identity())
for f in m1.frames:
if f.parent < legMaxId:
m2.addFrame(f)
g2 = pinocchio.GeometryModel()
for g in robot.visual_model.geometryObjects:
if g.parentJoint < legMaxId:
g2.addGeometryObject(g)
robot.model = m2
robot.data = m2.createData()
robot.visual_model = g2
robot.visual_data = pinocchio.GeometryData(g2)
# Load SRDF file
#q0 = example_robot_data.readParamsFromSrdf(robot.model, modelPath + SRDF_SUBPATH, False, False, 'standing')
robot.q0 = np.zeros(m2.nq + 1)
assert ((m2.rotorInertia[:m2.joints[1].nq] == 0.).all())
return robot
def initSolo():
robot = loadSolo(solo=False)
# Get robot model, data, and collision model
rmodel = robot.model
rdata = rmodel.createData()
gmodel = robot.collision_model
# Get the base_link and FL_UPPER_LEG meshes
base_link_geom = gmodel.getGeometryId("base_link_0")
leg_geom = gmodel.getGeometryId("FL_UPPER_LEG_0")
# Add the collision pair to the geometric model
gmodel.addCollisionPair(pio.CollisionPair(base_link_geom, leg_geom))
gdata = gmodel.createData()
return robot, rmodel, rdata, gmodel, gdata
def configure_simulation(dt, enableGUI):
global jointTorques
# Load the robot for Pinocchio
solo = loadSolo(True)
solo.initDisplay(loadModel=True)
# Start the client for PyBullet
if enableGUI:
physicsClient = p.connect(p.GUI)
else:
physicsClient = p.connect(p.DIRECT) # noqa
# p.GUI for graphical version
# p.DIRECT for non-graphical version
# Set gravity (disabled by default)
p.setGravity(0, 0, -9.81)
# Load horizontal plane for PyBullet
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.loadURDF("plane.urdf")
# Load the robot for PyBullet
robotStartPos = [0, 0, 0.35]
robotStartOrientation = p.getQuaternionFromEuler([0, 0, 0])
p.setAdditionalSearchPath("/opt/openrobots/share/example-robot-data/solo_description/robots")
robotId = p.loadURDF("solo.urdf", robotStartPos, robotStartOrientation)
# Set time step of the simulation
# dt = 0.001
p.setTimeStep(dt)
# realTimeSimulation = True # If True then we will sleep in the main loop to have a frequency of 1/dt
# Disable default motor control for revolute joints
revoluteJointIndices = [0, 1, 3, 4, 6, 7, 9, 10]
p.setJointMotorControlArray(robotId,
jointIndices=revoluteJointIndices,
controlMode=p.VELOCITY_CONTROL,
targetVelocities=[0.0 for m in revoluteJointIndices],
forces=[0.0 for m in revoluteJointIndices])
# Enable torque control for revolute joints
jointTorques = [0.0 for m in revoluteJointIndices]
p.setJointMotorControlArray(robotId, revoluteJointIndices, controlMode=p.TORQUE_CONTROL, forces=jointTorques)
# Compute one step of simulation for initialization
p.stepSimulation()
return robotId, solo, revoluteJointIndices
def test_controller(self):
solo = loadSolo(True)
solo.initViewer(loadModel=True)
q = solo.q0.copy()
qdot = np.zeros((solo.model.nv, 1))
dt = 1.0
t_simu = 0.0
qa_ref = solo.q0[7:]
qa_dot_ref = np.zeros((8, 1))
qa = q[7:]
qa_dot = qdot[6:]
expected_torques = np.matrix([[3.], [3.], [-3.], [3.], [-3.], [-3.],
[3.], [-3.]])
computed_torques = c_walking_IK(q, qdot, dt, solo, t_simu)
for i in range(8):
self.assertEqual(computed_torques[i, 0], expected_torques[i, 0])
import os
import sys
import time
import example_robot_data
import numpy as np
import crocoddyl
import pinocchio
from crocoddyl.utils.quadruped import SimpleQuadrupedalGaitProblem, plotSolution
WITHDISPLAY = 'display' in sys.argv or 'CROCODDYL_DISPLAY' in os.environ
WITHPLOT = 'plot' in sys.argv or 'CROCODDYL_PLOT' in os.environ
# Loading the solo model
solo = example_robot_data.loadSolo(False)
robot_model = solo.model
lims = robot_model.effortLimit
# 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 = pinocchio.utils.zero(robot_model.nv)
x0 = np.concatenate([q0, v0])
# Defining the CoM gait parameters
Jumping_gait = {
def __init__(self,
enableGepetto=False,
enableGUI=False,
enableGravity=True,
dt=1e-4):
self.solo = loadSolo(False)
self.enableGepetto = enableGepetto
if self.enableGepetto:
self.solo.initDisplay(loadModel=True)
# Start the client for PyBullet
self.enableGUI = enableGUI
if self.enableGUI:
self.physicsClient = p.connect(p.GUI)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
else:
self.physicsClient = p.connect(p.DIRECT) # noqa
# Set gravity (enabled by default)
self.enableGravity = enableGravity
if enableGravity:
p.setGravity(0, 0, -9.81)
else:
p.setGravity(0, 0, 0)
# Load horizontal plane for PyBullet
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.loadURDF("plane.urdf")
# Load the robot for PyBullet
robotStartPos = [0, 0, 0.4]
robotStartOrientation = p.getQuaternionFromEuler([0, 0, 0])
p.setAdditionalSearchPath(
"/opt/openrobots/share/example-robot-data/robots/solo_description/robots"
)
self.robotId = p.loadURDF("solo12.urdf", robotStartPos,
robotStartOrientation)
# Set time step of the simulation
self.dt = dt
p.setTimeStep(self.dt)
# Disable default motor control for revolute joints
self.revoluteJointIndices = [0, 1, 2, 4, 5, 6, 8, 9, 10, 12, 13, 14]
p.setJointMotorControlArray(
self.robotId,
jointIndices=self.revoluteJointIndices,
controlMode=p.VELOCITY_CONTROL,
targetVelocities=[0.0 for m in self.revoluteJointIndices],
forces=[0.0 for m in self.revoluteJointIndices])
# Enable torque control for revolute joints
jointTorques = np.zeros(len(self.revoluteJointIndices))
self.set_joint_torques(jointTorques)
# Compute one step of simulation for initialization
p.stepSimulation()
# Last state
self.last_state = self.get_q()
self.simulation_time = 0
import example_robot_data as robex
r = robex.loadSolo(False)
r.initViewer(loadModel=True)
gv = r.viewer.gui
# gv.deleteNode('world/pinocchio',True)
gv.createGroup('world/angmom')
for i in range(1, 5):
gv.deleteNode('world/angmom/w%d' % i, True)
gv.deleteNode('world/angmom/b%d' % i, True)
gv.addSphere('world/angmom/w%d' % i, .02, [1, 1, 1, 1])
gv.addSphere('world/angmom/b%d' % i, .02, [0, 0, 0, 1])
eps = 1e-3
gv.applyConfiguration('world/angmom/w1', [eps, eps, eps, 0, 0, 0, 1])
gv.applyConfiguration('world/angmom/w2', [-eps, -eps, eps, 0, 0, 0, 1])
gv.applyConfiguration('world/angmom/b1', [-eps, eps, eps, 0, 0, 0, 1])
gv.applyConfiguration('world/angmom/b2', [eps, -eps, eps, 0, 0, 0, 1])
gv.applyConfiguration('world/angmom/b3', [eps, eps, -eps, 0, 0, 0, 1])
gv.applyConfiguration('world/angmom/b4', [-eps, -eps, -eps, 0, 0, 0, 1])
gv.applyConfiguration('world/angmom/w3', [-eps, eps, -eps, 0, 0, 0, 1])
gv.applyConfiguration('world/angmom/w4', [eps, -eps, -eps, 0, 0, 0, 1])
gv.applyConfiguration('world/angmom', [0, 0, 1, 0, 0, 0, 1])
gv.refresh()
class Solo12Test(RobotTestCase):
RobotTestCase.ROBOT = example_robot_data.loadSolo(False)
RobotTestCase.NQ = 19
RobotTestCase.NV = 18
class SoloTest(RobotTestCase):
RobotTestCase.ROBOT = example_robot_data.loadSolo()
RobotTestCase.NQ = 15
RobotTestCase.NV = 14
def generateTrajectory(self, **kwargs):
import time
from .solo_tsid import SoloTSID
# Load parameters of the trajectory
self.setParametersFromDict(**kwargs)
dt = self.getParameter('dt')
param_kp = self.getParameter('kp')
param_kd = self.getParameter('kd')
param_vertVel = self.getParameter('verticalVelocity')
N_simu = self.getParameter('N_simu')
t0 = time.time()
if self.getParameter('debug'):
print("Computing Trajectory...")
self.printInfo()
# Initialize Viewer if needed
if self.getParameter('gepetto_viewer'):
solo12 = loadSolo(False)
solo12.initViewer(loadModel=True)
# Initialize TSID
tsid = SoloTSID()
tsid.setCOMRef(np.array([0.0, 0.0, -0.1]).T, np.zeros(3), np.zeros(3))
tsid.setBaseRef()
comObj1 = tsid.getCOM() + np.array([0.0, 0.0, -0.1]).T
comObj2 = tsid.getCOM() + np.array([0.0, 0.0, 0.09]).T
# Initalize trajectories
q = np.zeros((N_simu + 1, tsid.solo12_wrapper.nq))
v = np.zeros((N_simu + 1, tsid.solo12_wrapper.nv))
dv = np.zeros((N_simu + 1, tsid.solo12_wrapper.nv))
tau = np.zeros((N_simu + 1, tsid.solo12_wrapper.na))
gains = np.zeros((N_simu + 1, 2))
t_traj = np.arange(0, N_simu + 1) * self.getParameter('dt')
# Launch simu
t = 0.0
t_traj[0] = t
q[0, :], v[0, :] = tsid.q0, tsid.v0
gains[0, :] = np.array([param_kp, param_kd])
for i in range(N_simu - 2):
HQPData = tsid.formulation.computeProblemData(t, q[i, :], v[i, :])
sol = tsid.solver.solve(HQPData)
com = tsid.getCOM()
deltaCom1 = abs(com[2] - comObj1[2])
deltaCom2 = abs(com[2] - comObj2[2])
if deltaCom1 < 2e-2:
tsid.setCOMRef(
np.array([0.0, 0.0, 0.1]).T,
np.array([0.0, 0.0, param_vertVel]), np.zeros(3))
if deltaCom2 < 1e-2:
break
if sol.status != 0:
print(
"Time {0:0.3f} QP problem could not be solved! Error code: {1}"
.format(t, sol.status))
break
tau[i, :] = tsid.formulation.getActuatorForces(sol)
dv[i, :] = tsid.formulation.getAccelerations(sol)
# Numerical integration
q[i + 1, :], v[i + 1, :] = tsid.integrate_dv(
q[i, :], v[i, :], dv[i, :], dt)
t += dt
# Set the gains
gains[i + 1, :] = np.array([param_kp, param_kd])
if self.getParameter('gepetto_viewer'):
solo12.display(q[i, :])
time.sleep(1e-3)
q[i + 1, :], v[i + 1, :] = tsid.q0, tsid.v0
gains[i + 1, :] = np.array([param_kp, param_kd])
tau[i + 1, :] = np.zeros(tsid.solo12_wrapper.na)
# 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[:i + 1])
traj.addElement('q', q[:i + 1, 7:])
traj.addElement('q_dot', v[:i + 1, 6:])
traj.addElement('gains', gains[:i + 1, :])
traj.addElement('torques', tau[:i + 1, :])
return traj
def generateTrajectory(self, **kwargs):
import time
# Load parameters of the trajectory
self.setParametersFromDict(**kwargs)
# params: tf, dt, kp, kd, kps, kds, debug, init_reversed, max_error
q_traj = []
qdot_traj = []
gains = []
t0 = time.time()
if self.getParameter('debug'):
print("Computing Trajectory...")
self.printInfo()
solo = loadSolo(False)
# Compute feet trajectory
t_traj, feet_traj, gains_traj = self.getParameter('feet_traj_func')(
**self.getParameter('feet_traj_params'))
# Place the robot in a regular configuration
solo.q0[2] = 0.
for i in range(4):
sign = 1 if self.getParameter('init_reversed') else -1
if i < 2:
solo.q0[7 + 3 * i + 1] = +sign * pi / 4
solo.q0[7 + 3 * i + 2] = -sign * pi / 2
else:
solo.q0[7 + 3 * i + 1] = -sign * pi / 4
solo.q0[7 + 3 * i + 2] = +sign * pi / 2
q = solo.q0.copy()
q_dot = np.zeros((solo.model.nv, 1))
# Reach the initial position first
step = 0
while True:
q, q_dot, err = self.kinInv_3D(q, q_dot, solo, feet_traj[0])
if err < self.getParameter('max_init_error'):
break
step += 1
if step > self.getParameter('max_init_iterations'):
print("Unable to reach the first position.")
return None
# Run the trajectory definition
flag_errTooHigh = False
maxE = 0
for i, t in enumerate(t_traj):
q, q_dot, err = self.kinInv_3D(q, q_dot, solo, feet_traj[i])
q_traj.append(q)
qdot_traj.append(q_dot)
gains.append(gains_traj[i])
if err > self.getParameter('max_kinv_error'):
flag_errTooHigh = True
if err > maxE:
maxE = err
# If a position wasn't reachead, print it
if flag_errTooHigh:
print(
"The error was pretty high. Maybe the trajectory is trop ambitious (maxE=",
maxE,
")",
sep='')
# Convert to numpy arrays
q_traj = np.array(q_traj)
qdot_traj = np.array(qdot_traj)
gains = np.array(gains)
# Initialize angles at zero
offsets = np.zeros(q_traj.shape[1])
for i in range(q_traj.shape[1]):
while abs(q_traj[0][i] - offsets[i] * 2 * pi) > pi:
if q_traj[0][i] > 0:
offsets[i] += 1
else:
offsets[i] -= 1
for i in range(q_traj.shape[0]):
q_traj[i] = q_traj[i] - 2 * pi * offsets
# 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', q_traj[:, 7:])
traj.addElement('q_dot', qdot_traj[:, 6:])
traj.addElement('gains', gains)
return traj
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
# Generate the trajectory
traj = traj_gen.generateTrajectory()
# Save the trajectory
if SAVE:
traj.saveTrajectory(file_name)
print("Trajectory saved as:", file_name)
# Plot the trajectory
if PLOT:
traj.plotTrajectory(show_gains=True, show_all=True)
# Check security
secu = SecurityChecker()
secu.check_trajectory(traj)
secu.show_results(show_all=False)
# Display the generated trajectory in Gepetto-Gui
if DISPLAY:
solo = loadSolo(False)
solo.initViewer(loadModel=True)
t0 = time.time()
for i in range(traj.getSize()):
q = np.concatenate((np.array([0, 0, 0.4, 0, 0, 0,
1]), traj.getElement('q', i)))
solo.display(q)
while (time.time() - t0 < traj.getElement('t', i)):
time.sleep(0.00001)