import pinocchio as se3
from pinocchio.robot_wrapper import RobotWrapper
from pinocchio.utils import *
from math import pi,sqrt,cos,sin
from hrp2_reduced import Hrp2Reduced
import time
from simu import Simu
from quadprog import solve_qp
pkg = '/home/nmansard/src/sot_versions/groovy/ros/stacks/hrp2/'
urdf = '/home/nmansard/src/pinocchio/pinocchio/models/hrp2014.urdf'
robot = Hrp2Reduced(urdf,[pkg],loadModel=True)
robot.display(robot.q0)
robot.viewer.setCameraTransform(0,[1.9154722690582275,
-0.2266872227191925,
0.1087859719991684,
0.5243823528289795,
0.518651008605957,
0.4620114266872406,
0.4925136864185333])
simu = Simu(robot,dt=1e-3,ndt=5)
NQ,NV,NB,RF,LF,RK,LK = simu.NQ,simu.NV,simu.NB,simu.RF,simu.LF,simu.RK,simu.LK
def loop(q,v,niter,ndt=None,dt=None,tsleep=.9,fdisplay=1):
t0 = time.time()
if dt is not None: simu.dt = dt
if ndt is not None: simu.ndt = ndt
robot.display(q)
for i in range(niter):
q,v = simu(q,v,control(q,v))
except AssertionError:
print(
"Solution violate the desired feet acceleration and so the CoM snap {}"
).format(np.linalg.norm(feet_a_des - (Jc * dv + dJcdq)))
#external forces should be zero
assert (isapprox((M * dv + h - Jc.T * fc)[:3], np.zeros([3, 1])))
return np.vstack([zero(3), tau])
if __name__ == "__main__":
'''benchmark TSID'''
from hrp2_reduced import Hrp2Reduced
import time
from path import pkg, urdf
np.set_printoptions(precision=3)
np.set_printoptions(linewidth=200)
robot = Hrp2Reduced(urdf, [pkg], loadModel=True, useViewer=False)
tsid = TsidFlexibleContact(robot,
Ky=23770.,
Kz=239018.,
w_post=0e-3,
Kp_post=1.,
Kp_com=1.,
Kd_com=1.,
Ka_com=1.,
Kj_com=1.)
niter = 3000
t0 = time.time()
for i in range(niter):
tsid.solve(robot.q0, np.zeros([7, 1]),
np.matrix([0., 0., 0., 0.]).T,
np.matrix([0., 0., 0., 0.]).T)