def __call__(self, t):
if t < 0.:
print("Trajectory called with negative time.")
t = 0.
elif t > self._length:
print("Trajectory called after final time.")
t = self._length
return effectorPositionFromHPPPath(self._fb, self._problem,
self._eeName, self._pid, t)
def computeDistanceCostMatrices(fb, pathId, pData, T, eeName, numPoints=50):
problem = fb.client.problem
# build a matrice 3xnumPoints by sampling the given path :
step = problem.pathLength(pathId) / (numPoints - 1)
pts = np.zeros([3, numPoints])
for i in range(numPoints):
p = effectorPositionFromHPPPath(fb, problem, eeName, pathId, float(step * i))
pts[:, i] = p
return bezier_com.computeEndEffectorDistanceCost(pData, T, numPoints, pts)