import numpy as np
import os
import pinocchio as pin
from mlp.utils.util import discretizeCurve, discretizeSE3CurveTranslation, discretizeDerivateCurve
import matplotlib
matplotlib.use("Qt4agg")
import matplotlib.pyplot as plt
from multiprocessing import Process
from mlp.utils.requirements import Requirements
pin.switchToNumpyArray()
plt.ioff()
def show(plt):
plt.show()
def show_non_blocking(plt):
p = Process(target=show, args=([plt]))
p.start()
def addVerticalLineContactSwitch(cs, plt, linestyle="-.", color='k'):
for phase in cs.contactPhases:
plt.axvline(phase.timeFinal, linestyle=linestyle, color=color)
def plotEffectorTrajectoryWithReference(cs_ref, cs, dt):
labels = [
"x (m)", "y (m)", "z (m)", "dx (m/s)", "dy (m/s)", "dz (m/s)",
"ddx (m/s^2)", "ddy (m/s^2)", "ddz (m/s^2)"
import sys
from os.path import dirname, exists, join
import numpy as np
import pinocchio
from pinocchio.robot_wrapper import RobotWrapper
pinocchio.switchToNumpyArray()
def getModelPath(subpath, printmsg=False):
paths = [
join(dirname(dirname(dirname(dirname(__file__)))), 'robots'),
join(dirname(dirname(dirname(__file__))), 'robots')
]
try:
from .path import EXAMPLE_ROBOT_DATA_MODEL_DIR
paths.append(EXAMPLE_ROBOT_DATA_MODEL_DIR)
except ImportError:
pass
paths += [
join(p, '../../../share/example-robot-data/robots') for p in sys.path
]
for path in paths:
if exists(join(path, subpath.strip('/'))):
if printmsg:
print("using %s as modelPath" % path)
return path
raise IOError('%s not found' % subpath)
