nbSamples = 10000
ps = ProblemSolver(fullBody)
r = Viewer(ps)
q_init = hyq_ref[:]
r(q_init)
from hppy_ik import *
from numpy import array
from numpy.linalg import norm
import eigenpy
eigenpy.switchToNumpyArray()
__EPS = 1e-4
# load robot
packageName = "hyq_description"
meshPackageName = "hyq_description"
rootJointType = "freeflyer"
urdfName = "hyq"
urdfSuffix = ""
srdfSuffix = ""
robotName = "hyq"
hyq = IkHelper(robotName, rootJointType, packageName, urdfName, urdfSuffix,
srdfSuffix)
print(cmd2)
ipython.magic(cmd2)
print("\n")
eigenpy.switchToNumpyMatrix()
print("----------------------")
print("switch to numpy matrix")
print("----------------------")
print("\n")
cmd3 = "timeit quat.coeffs()"
print(cmd3)
ipython.magic(cmd3)
print("\n")
eigenpy.switchToNumpyArray()
print("---------------------")
print("switch to numpy array")
print("---------------------")
print("\n")
cmd4 = "timeit quat.coeffs()"
print(cmd4)
ipython.magic(cmd4)
print("\n")
cmd5 = "timeit np.asmatrix(quat.coeffs())"
print(cmd5)
ipython.magic(cmd5)
print("\n")
