def createSoloTBWrapper():
'''
Returns a RobotWrapper with a monoped inside.
'''
rmodel = createSoloTB()
rw = pinocchio.RobotWrapper(rmodel,visual_model=None,collision_model=None)
return rw
def createMonopedWrapper(nbJoint, linkLength=1.0, floatingMass=1.0, linkMass=1.0):
'''
Returns a RobotWrapper with a monoped inside.
'''
rmodel = createMonoped(nbJoint,linkLength, floatingMass, linkMass)
rw = pinocchio.RobotWrapper(rmodel,visual_model=None,collision_model=None)
return rw
def loadSoloLeg(initViewer=False):
# Load Solo8.
URDF_FILENAME = "solo.urdf"
SRDF_FILENAME = "solo.srdf"
SRDF_SUBPATH = "/solo_description/srdf/" + SRDF_FILENAME
URDF_SUBPATH = "/solo_description/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
robot = pio.RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH,
[modelPath])
readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False,
"standing")
# Create Solo-leg model from Solo8
lock = list(range(3, 9))
rmodel = pio.buildReducedModel(robot.model, lock, robot.q0)
rdata = rmodel.createData()
# Create solo-leg geometry model (visuals and collisions
reduceGeomModel(robot.model, robot.data, rmodel, rdata, robot.visual_model,
robot.visual_data, robot.q0, robot.q0[:2])
reduceGeomModel(robot.model, robot.data, rmodel, rdata,
robot.collision_model, robot.collision_data)
# Create Solo robot-wrapper
rw = pio.RobotWrapper(rmodel,
collision_model=robot.collision_model,
visual_model=robot.visual_model)
if initViewer:
rw.initViewer(loadModel=True)
for g in rw.visual_model.geometryObjects:
if g.parentJoint == 0:
rw.viewer.gui.setVisibility(
'world/pinocchio/visuals/' + g.name, 'OFF')
return rw
def createPendulumWrapper(nbJoint, initViewer=True):
'''
Returns a RobotWrapper with a N-pendulum inside.
'''
gv = GepettoViewerServer() if initViewer else None
rmodel, gmodel = createPendulum(nbJoint, viewer=gv)
rw = pio.RobotWrapper(rmodel, visual_model=gmodel, collision_model=gmodel)
if initViewer: rw.initViewer(loadModel=True)
return rw
DELTA_FOOT_Z = 0.1 # desired elevation of right foot in z direction
dt = 0.001 # controller time step
PRINT_N = 500 # print every PRINT_N time steps
DISPLAY_N = 25 # update robot configuration in viwewer every DISPLAY_N time steps
N_SIMULATION = 4000 # number of time steps simulated
filename = str(os.path.dirname(os.path.abspath(__file__)))
path = filename + '/../models/romeo'
urdf = path + '/urdf/romeo.urdf'
vector = se3.StdVec_StdString()
vector.extend(item for item in path)
robot = tsid.RobotWrapper(urdf, vector, se3.JointModelFreeFlyer(), False)
srdf = path + '/srdf/romeo_collision.srdf'
# for gepetto viewer .. but Fix me!!
robot_display = se3.RobotWrapper(urdf, [path, ], se3.JointModelFreeFlyer())
l = commands.getstatusoutput("ps aux |grep 'gepetto-gui'|grep -v 'grep'|wc -l")
if int(l[1]) == 0:
os.system('gepetto-gui &')
time.sleep(1)
cl = gepetto.corbaserver.Client()
gui = cl.gui
robot_display.initDisplay(loadModel=True)
q = se3.getNeutralConfigurationFromSrdf(robot.model(), srdf, False)
q[2] += 0.84
v = np.matrix(np.zeros(robot.nv)).transpose()
robot_display.displayCollisions(False)
robot_display.displayVisuals(True)
else:
nu = se3.log(Mrh.inverse() * Mdes)
Jrh = robot.Jrh(q)
qdot = npl.pinv(Jrh) * nu.vector()
robot.increment(q, qdot * 5e-2)
robot.display(q)
print("Residuals = ",
npl.norm(se3.log(robot.Mrh(q).inverse() * Mdes).vector()))
# --- MAIN ------------------------------------------------------
if __name__ == '__main__':
import pinocchio as se3
from pinocchio.utils import *
robot = se3.RobotWrapper('../../models/romeo.urdf')
robot.initDisplay()
kineinv_dim1(robot.q0, 2.0)
kineinv_dim3(robot.q0, np.matrix([2.0, 0.5, 1.0]).T)
kineinv_dim6(robot.q0,
se3.SE3(eye(3),
np.matrix([2.0, 0.5, 1.0]).T),
straight=False)
kineinv_dim6(robot.q0,
se3.SE3(eye(3),
np.matrix([2.0, 0.5, 1.0]).T),
straight=True)