def test_serialization(self):
c = curve_constraints(3)
c.init_vel = array([[0., 1., 1.]]).transpose()
c.end_vel = array([[0., -1., 1.]]).transpose()
c.init_acc = array([[0., 1., -1.]]).transpose()
c.end_acc = array([[0., 100., 1.]]).transpose()
c.init_jerk = array([[2., 4., 1.]]).transpose()
c.end_jerk = array([[-1., 2., 7.]]).transpose()
c.saveAsText("curve_constraints.txt")
c.saveAsXML("curve_constraints.xml", "curve_constraints")
c.saveAsBinary("curve_constraints")
c_txt = curve_constraints()
c_txt.loadFromText("curve_constraints.txt")
self.assertEqual(c, c_txt)
c_xml = curve_constraints()
c_xml.loadFromXML("curve_constraints.xml", "curve_constraints")
self.assertEqual(c, c_xml)
c_bin = curve_constraints()
c_bin.loadFromBinary("curve_constraints")
self.assertEqual(c, c_bin)
c_pickled = pickle.dumps(c)
c_from_pickle = pickle.loads(c_pickled)
self.assertEqual(c_from_pickle, c)
def test_operator_equal(self):
c = curve_constraints(3)
c.init_vel = array([[0., 1., 1.]]).transpose()
c.end_vel = array([[0., -1., 1.]]).transpose()
c.init_acc = array([[0., 1., -1.]]).transpose()
c.end_acc = array([[0., 100., 1.]]).transpose()
c.init_jerk = array([[2., 4., 1.]]).transpose()
c.end_jerk = array([[-1., 2., 7.]]).transpose()
c2 = curve_constraints(3)
c2.init_vel = array([[0., 1., 1.]]).transpose()
c2.end_vel = array([[0., -1., 1.]]).transpose()
c2.init_acc = array([[0., 1., -1.]]).transpose()
c2.end_acc = array([[0., 100., 1.]]).transpose()
c2.init_jerk = array([[2., 4., 1.]]).transpose()
c2.end_jerk = array([[-1., 2., 7.]]).transpose()
self.assertTrue(c == c2)
c2.init_vel = array([[1., 1., 1.]]).transpose()
self.assertTrue(c != c2)
def computeProblemConstraints(pData, fullBody, pathId, t, eeName, viewer):
groupName = "constraints_" + str(pathId)
if DISPLAY_CONSTRAINTS:
viewer.client.gui.createGroup(groupName)
pDef = curves.problemDefinition()
# set up problem definition :
pDef.flag = int(curves.constraint_flag.INIT_POS) \
| int(curves.constraint_flag.INIT_VEL) \
| int(curves.constraint_flag.INIT_ACC)\
| int(curves.constraint_flag.INIT_JERK) \
| int(curves.constraint_flag.END_POS) \
| int(curves.constraint_flag.END_VEL) \
| int(curves.constraint_flag.END_ACC) \
| int(curves.constraint_flag.END_JERK)
pDef.costFlag = curves.derivative_flag.VELOCITY
pDef.start = pData.c0_
pDef.end = pData.c1_
curveConstraints = curves.curve_constraints()
curve_constraints.init_vel = pData.dc0_
curve_constraints.init_acc = pData.ddc0_
curve_constraints.init_jerk = pData.j0_
curve_constraints.end_vel = pData.dc1_
curve_constraints.end_acc = pData.ddc1_
curve_constraints.end_jerk = pData.j1_
pDef.curveConstraints = curveConstraints
# get all the waypoints from the limb-rrt
wps, t_paths = fullBody.client.problem.getWaypoints(pathId)
wps, t_paths = filterWPs(fullBody, eeName, wps, t_paths)
# approximate the switching times (infos from limb-rrt)
if len(t_paths) > 2:
splits = []
# ratio between the imposed time of the bezier curve (t) and the "time" (a pseudo distance) of the solution of the rrt
t_ratio = t / t_paths[-1]
for i in range(1, len(t_paths) - 1):
ti = t_paths[i] * t_ratio
if ti > t:
ti = t
splits += [ti]
if len(splits) > 1:
if splits[-1] == splits[-2]:
print(
"Error in bezier_constrained : two adjacent constrained have the same switch time !!"
)
if VERBOSE:
print("number of switch between constraints : ", len(splits))
print("splits timings : ", splits)
pDef.splits = np.array([splits]).T
else:
if VERBOSE:
print("Only one constraint set for the whole trajectory.")
# compute constraints around each line of the limb-rrt solution :
q_from = wps[0]
p_from = contactPlacementFromConfig(fullBody, q_from, eeName)
# debug test :
#p_from.translation = pData.c0_ # not in the right frame !
for i in range(1, len(wps)):
q_to = wps[i]
p_to = contactPlacementFromConfig(fullBody, q_to, eeName)
#if i == len(wps)-1 : # debug test
# p_from.translation = pData.c1_
A, b = computeInequalitiesAroundLine(fullBody, p_from, p_to, eeName,
groupName, viewer)
if VERBOSE:
print("Inequalities computed.")
pDef.addInequality(A, b)
p_from = p_to.copy()
if DISPLAY_CONSTRAINTS:
viewer.client.gui.addToGroup(groupName, viewer.sceneName)
viewer.client.gui.refresh()
return pDef