1 import pinocchio
as pin
4 model = pin.buildSampleModelHumanoid()
5 model.lowerPositionLimit[:7] = -np.ones(7)
6 model.upperPositionLimit[:7] = +np.ones(7)
8 pool = pin.ModelPool(model)
10 num_threads = pin.omp_get_max_threads()
12 q = np.empty((model.nq,batch_size))
13 for k
in range(batch_size):
14 q[:,k] = pin.randomConfiguration(model)
16 v = np.zeros((model.nv,batch_size))
17 a = np.zeros((model.nv,batch_size))
18 tau = np.zeros((model.nv,batch_size))
20 print(
"num_threads: {}".format(num_threads))
21 print(
"batch_size: {}".format(batch_size))
24 res_rnea = np.empty((model.nv,batch_size))
25 pin.rnea(num_threads,pool,q,v,a,res_rnea)
26 res_rnea2 = pin.rnea(num_threads,pool,q,v,a)
29 res_aba = np.empty((model.nv,batch_size))
30 pin.aba(num_threads,pool,q,v,tau,res_aba)
31 res_aba2 = pin.aba(num_threads,pool,q,v,tau)