def generatePredefLandingTakeoff(time_interval,placement_init,placement_end):
t_total = time_interval[1]-time_interval[0] - 2*cfg.EFF_T_DELAY
#print "Generate Bezier Traj :"
#print "placement Init = ",placement_init
#print "placement End = ",placement_end
#print "time interval = ",time_interval
# generate two curves for the takeoff/landing :
# generate a bezier curve for the middle part of the motion :
bezier_takeoff = buildPredefinedInitTraj(placement_init,t_total)
bezier_landing = buildPredefinedFinalTraj(placement_end,t_total)
t_middle = (t_total - (2.*cfg.EFF_T_PREDEF))
assert t_middle >= 0.1 and "Duration of swing phase too short for effector motion. Change the values of predef motion for effector or the duration of the contact phase. "
curves = []
# create polybezier with concatenation of the 3 (or 5) curves :
# create constant curve at the beginning and end for the delay :
if cfg.EFF_T_DELAY > 0 :
bezier_init_zero=bezier(bezier_takeoff(0),cfg.EFF_T_DELAY)
curves.append(bezier_init_zero)
curves.append(bezier_takeoff)
curves.append(bezier(np.matrix(np.zeros(3)).T,t_middle)) # placeholder only
curves.append(bezier_landing)
if cfg.EFF_T_DELAY > 0 :
curves.append(bezier(bezier_landing(bezier_landing.max()),cfg.EFF_T_DELAY))
pBezier = PolyBezier(curves)
return pBezier
def buildPredefinedFinalTraj(placement, t_total):
p_off, v_off, a_off = computePredefConstants(t_total)
normal = placement.rotation * np.matrix([0, 0, 1]).T
#print "normal used for landing : ",normal.T
#print "offset used : ",p_off
c0 = placement.translation.copy()
c1 = placement.translation.copy()
c0 += p_off * normal
#print "landing part, c0 : ",c0.T
#print "landing part, c1 : ",c1.T
dc1 = np.matrix(np.zeros(3)).T
#dc0 = v_off * normal
ddc1 = np.matrix(np.zeros(3)).T
#ddc0 = a_off * normal
#create wp :
n = 4.
wps = np.matrix(np.zeros(([3, int(n + 1)])))
T = cfg.EFF_T_PREDEF
# constrained init pos and final pos. final vel, acc and jerk = 0
wps[:, 0] = (c0)
#c0
wps[:, 1] = ((n * n * c1 - n * c1 - 3 * n * dc1 * T + 3 * dc1 * T +
3 * ddc1 * T * T) / (n * (n - 1)))
# j1
wps[:, 2] = (
(n * n * c1 - n * c1 - 2 * n * dc1 * T + 2 * dc1 * T + ddc1 * T * T) /
(n * (n - 1)))
#ddc1 * T ??
wps[:, 3] = ((-dc1 * T / n) + c1)
#dc1
wps[:, 4] = (c1)
#c1
return bezier(wps, T)
def buildPredefinedInitTraj(placement, t_total):
p_off, v_off, a_off = computePredefConstants(t_total)
normal = placement.rotation * np.matrix([0, 0, 1]).T
#print "normal used for takeoff : ",normal.T
#print "offset used : ",p_off
c0 = placement.translation.copy()
c1 = placement.translation.copy()
c1 += p_off * normal
#print "takeoff part, c0 : ",c0.T
#print "takeoff part, c1 : ",c1.T
dc0 = np.matrix(np.zeros(3)).T
#dc1 = v_off * normal
ddc0 = np.matrix(np.zeros(3)).T
#ddc1 = a_off * normal
#create wp :
n = 4.
wps = np.matrix(np.zeros(([3, int(n + 1)])))
T = cfg.EFF_T_PREDEF
# constrained init pos and final pos. Init vel, acc and jerk = 0
wps[:, 0] = (c0)
# c0
wps[:, 1] = ((dc0 * T / n) + c0)
#dc0
wps[:, 2] = ((n * n * c0 - n * c0 + 2. * n * dc0 * T - 2. * dc0 * T +
ddc0 * T * T) / (n * (n - 1.)))
#ddc0 // * T because derivation make a T appear
wps[:, 3] = ((n * n * c0 - n * c0 + 3. * n * dc0 * T - 3. * dc0 * T +
3. * ddc0 * T * T) / (n * (n - 1.)))
#j0 = 0
wps[:, 4] = (c1)
#c1
return bezier(wps, T)
def generatePredefMiddle(bezier_takeoff, bezier_landing, T):
c0 = bezier_takeoff(bezier_takeoff.max())
dc0 = bezier_takeoff.derivate(bezier_takeoff.max(), 1)
ddc0 = bezier_takeoff.derivate(bezier_takeoff.max(), 2)
j0 = bezier_takeoff.derivate(bezier_takeoff.max(), 3)
c1 = bezier_landing(0)
dc1 = bezier_landing.derivate(0, 1)
ddc1 = bezier_landing.derivate(0, 2)
j1 = bezier_landing.derivate(0, 3)
n = 7
wps = np.matrix(np.zeros(([3, int(n + 1)])))
wps[:, 0] = (c0)
# c0
wps[:, 1] = ((dc0 * T / n) + c0)
#dc0
wps[:, 2] = ((n * n * c0 - n * c0 + 2. * n * dc0 * T - 2. * dc0 * T +
ddc0 * T * T) / (n * (n - 1.)))
#ddc0 // * T because derivation make a T appear
wps[:,
3] = ((n * n * c0 - n * c0 + 3. * n * dc0 * T - 3. * dc0 * T +
3. * ddc0 * T * T + j0 * T * T * T / (n - 2)) / (n * (n - 1.)))
#j0
wps[:, 4] = ((n * n * c1 - n * c1 - 3 * n * dc1 * T + 3 * dc1 * T +
3 * ddc1 * T * T - j1 * T * T * T / (n - 2)) / (n * (n - 1)))
# j1
wps[:, 5] = (
(n * n * c1 - n * c1 - 2 * n * dc1 * T + 2 * dc1 * T + ddc1 * T * T) /
(n * (n - 1)))
#ddc1 * T ??
wps[:, 6] = ((-dc1 * T / n) + c1)
#dc1
wps[:, 7] = (c1)
#c1
return bezier(wps, T)
def displayBezierWaypoints(r,
wp,
step=0.001,
color=[0.85, 0.75, 0.15, 1.0],
name=None):
waypoints = matrix(wp).transpose()
curve = bezier(waypoints)
displayBezierCurve(r, curve, step, color, name)
return curve
def generateSmoothBezierTrajWithoutPredef(time_interval, placement_init,
placement_end):
t_tot = time_interval[1] - time_interval[0]
wps = np.matrix(np.zeros([3, 9]))
for i in range(4): # init position. init vel,acc and jerk == 0
wps[:, i] = placement_init.translation.copy()
# compute mid point (average and offset along z)
wps[:, 4] = (placement_init.translation + placement_end.translation) / 2.
wps[2, 4] += cfg.p_max
for i in range(5, 9): # final position. final vel,acc and jerk == 0
wps[:, i] = placement_end.translation.copy()
pBezier = PolyBezier(bezier(wps, t_tot))
ref_traj = trajectories.BezierTrajectory(pBezier, placement_init,
placement_end, time_interval)
return ref_traj
try:
res = quadprog_solve_qp(P, q, G, h)
solved = True
except ValueError, e:
print "Quadprog error : "
print e.message
raise ValueError(
"Quadprog failed to solve QP for optimized limb-RRT end-effector trajectory, for try number "
+ str(numTry))
# build a bezier curve from the result of quadprog :
vars = np.split(res, numVars)
wps = bezier_com.computeEndEffectorConstantWaypoints(
pData, t_middle) # one wp per column
id_firstVar = 4 # depend on the flag defined above, but for end effector we always use this ones ...
i = id_firstVar
for x in vars:
wps[:, i] = x
i += 1
bezier_middle = bezier(wps, t_middle)
# create concatenation with takeoff/landing
curves = predef_curves.curves[::]
curves[id_middle] = bezier_middle
pBezier = PolyBezier(curves)
if VERBOSE:
print "time interval = ", time_interval[1] - time_interval[0]
print "polybezier length = ", pBezier.max()
ref_traj = trajectories.BezierTrajectory(pBezier, placement_init,
placement_end, time_interval)
return ref_traj
def test_spline(self):
waypoints = array([[1., 2., 3.]]).T
a = bezier(waypoints, 2.)
t = 0.
while t < 2.:
assert_allclose(a(t), array([1., 2., 3.]).T)
t += 0.1
waypoints = array([[1., 2., 3.], [4., 5., 6.]]).T
waypoints6 = array([[1., 2., 3., 7., 5., 5.], [4., 5., 6., 4., 5.,
6.]]).T
time_waypoints = array([0., 1.]).T
# testing bezier curve
a = bezier6(waypoints6)
a = bezier(waypoints, 3.)
self.assertEqual(a.degree, a.nbWaypoints - 1)
a.min()
a.max()
a(0.4)
assert_allclose(a.derivate(0.4, 0), a(0.4))
a.derivate(0.4, 2)
a = a.compute_derivate(100)
prim = a.compute_primitive(1)
for i in range(10):
t = float(i) / 10.
assert_allclose(a(t), prim.derivate(t, 1))
assert_allclose(prim(0), array([0., 0., 0.]).T)
prim = a.compute_primitive(2)
for i in range(10):
t = float(i) / 10.
assert_allclose(a(t), prim.derivate(t, 2), atol=1e-20)
assert_allclose(prim(0), array([0., 0., 0.]).T)
waypoints = array([[1., 2., 3.], [4., 5., 6.], [4., 5., 6.],
[4., 5., 6.], [4., 5., 6.]]).T
a0 = bezier(waypoints)
a1 = bezier(waypoints, 3.)
prim0 = a0.compute_primitive(1)
prim1 = a1.compute_primitive(1)
for i in range(10):
t = float(i) / 10.
assert_allclose(a0(t), a1(3 * t))
assert_allclose(a0.derivate(t, 1), a1.derivate(3 * t, 1) * 3.)
assert_allclose(a0.derivate(t, 2), a1.derivate(3 * t, 2) * 9.)
assert_allclose(prim0(t), prim1(t * 3) / 3.)
assert_allclose(prim(0), array([0., 0., 0.]).T)
assert_allclose(prim(0), array([0., 0., 0.]))
# testing bezier with constraints
c = curve_constraints()
c.init_vel = array([0., 1., 1.]).T
c.end_vel = array([0., 1., 1.]).T
c.init_acc = array([0., 1., -1.]).T
c.end_acc = array([0., 100., 1.]).T
waypoints = array([[1., 2., 3.], [4., 5., 6.]]).T
a = bezier(waypoints, c)
assert_allclose(a.derivate(0, 1), c.init_vel)
assert_allclose(a.derivate(1, 2), c.end_acc)
# testing polynom function
a = polynom(waypoints)
a = polynom(waypoints, -1., 3.)
a.min()
a.max()
a(0.4)
assert_allclose(a.derivate(0.4, 0), a(0.4))
a.derivate(0.4, 2)
# testing exact_cubic function
a = exact_cubic(waypoints, time_waypoints)
a.min()
a.max()
a(0.4)
assert_allclose(a.derivate(0.4, 0), a(0.4))
a.derivate(0.4, 2)
# testing spline_deriv_constraints
c = curve_constraints()
c.init_vel
c.end_vel
c.init_acc
c.end_acc
c.init_vel = array([0., 1., 1.]).T
c.end_vel = array([0., 1., 1.]).T
c.init_acc = array([0., 1., 1.]).T
c.end_acc = array([0., 1., 1.]).T
a = spline_deriv_constraint(waypoints, time_waypoints)
a = spline_deriv_constraint(waypoints, time_waypoints, c)
# converting bezier to polynom
a = bezier(waypoints)
a_pol = from_bezier(a)
assert_allclose(a(0.3), a_pol(0.3))
def test_spline(self):
waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).T
waypoints6 = matrix([[1., 2., 3., 7., 5., 5.], [4., 5., 6., 4., 5., 6.]]).T
time_waypoints = matrix([0., 1.]).T
# testing bezier curve
a = bezier6(waypoints6)
a = bezier(waypoints, 3.)
self.assertEqual(a.degree, a.nbWaypoints - 1)
a.min()
a.max()
a(0.4)
assert_allclose(a.derivate(0.4, 0), a(0.4))
a.derivate(0.4, 2)
a = a.compute_derivate(100)
prim = a.compute_primitive(1)
for i in range(10):
t = float(i) / 10.
assert_allclose(a(t), prim.derivate(t, 1))
assert_allclose(prim(0), matrix([0., 0., 0.]).T)
prim = a.compute_primitive(2)
for i in range(10):
t = float(i) / 10.
assert_allclose(a(t), prim.derivate(t, 2), atol=1e-20)
assert_allclose(prim(0), matrix([0., 0., 0.]).T)
waypoints = matrix([[1., 2., 3.], [4., 5., 6.], [4., 5., 6.], [4., 5., 6.], [4., 5., 6.]]).T
a0 = bezier(waypoints)
a1 = bezier(waypoints, 3.)
prim0 = a0.compute_primitive(1)
prim1 = a1.compute_primitive(1)
for i in range(10):
t = float(i) / 10.
assert_allclose(a0(t), a1(3 * t))
assert_allclose(a0.derivate(t, 1), a1.derivate(3 * t, 1) * 3.)
assert_allclose(a0.derivate(t, 2), a1.derivate(3 * t, 2) * 9.)
assert_allclose(prim0(t), prim1(t * 3) / 3.)
assert_allclose(prim(0), matrix([0., 0., 0.]).T)
with self.assertRaises(AssertionError):
assert_allclose(prim(0), matrix([0., 0., 0.]))
# testing bezier with constraints
c = curve_constraints()
c.init_vel = matrix([0., 1., 1.]).T
c.end_vel = matrix([0., 1., 1.]).T
c.init_acc = matrix([0., 1., -1.]).T
c.end_acc = matrix([0., 100., 1.]).T
waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).T
a = bezier(waypoints, c)
assert_allclose(a.derivate(0, 1), c.init_vel)
assert_allclose(a.derivate(1, 2), c.end_acc)
# testing polynom function
a = polynom(waypoints)
a = polynom(waypoints, -1., 3.)
a.min()
a.max()
a(0.4)
assert_allclose(a.derivate(0.4, 0), a(0.4))
a.derivate(0.4, 2)
# testing exact_cubic function
a = exact_cubic(waypoints, time_waypoints)
a.min()
a.max()
a(0.4)
assert_allclose(a.derivate(0.4, 0), a(0.4))
a.derivate(0.4, 2)
# testing spline_deriv_constraints
c = curve_constraints()
c.init_vel
c.end_vel
c.init_acc
c.end_acc
c.init_vel = matrix([0., 1., 1.]).T
c.end_vel = matrix([0., 1., 1.]).T
c.init_acc = matrix([0., 1., 1.]).T
c.end_acc = matrix([0., 1., 1.]).T
a = spline_deriv_constraint(waypoints, time_waypoints)
a = spline_deriv_constraint(waypoints, time_waypoints, c)
# converting bezier to polynom
a = bezier(waypoints)
a_pol = from_bezier(a)
assert_allclose(a(0.3), a_pol(0.3))
def generateConstrainedBezierTraj(time_interval,
placement_init,
placement_end,
numTry,
q_t=None,
phase_previous=None,
phase=None,
phase_next=None,
fullBody=None,
eeName=None,
viewer=None):
if numTry == 0:
return generateSmoothBezierTraj(time_interval, placement_init,
placement_end)
else:
if not q_t or not phase_previous or not phase or not phase_next or not fullBody or not eeName:
raise ValueError(
"Cannot compute LimbRRTOptimizedTraj for try >= 1 without optionnal arguments"
)
predef_curves = generatePredefBeziers(time_interval, placement_init,
placement_end)
bezier_takeoff = predef_curves.curves[predef_curves.idFirstNonZero()]
bezier_landing = predef_curves.curves[predef_curves.idLastNonZero()]
id_middle = int(math.floor(len(predef_curves.curves) / 2.))
pos_init = bezier_takeoff(bezier_takeoff.max())
pos_end = bezier_landing(0)
if VERBOSE:
print "bezier takeoff end : ", pos_init
print "bezier landing init : ", pos_end
t_begin = predef_curves.times[id_middle]
t_middle = predef_curves.curves[id_middle].max()
t_end = t_begin + t_middle
if VERBOSE:
print "t begin : ", t_begin
print "t end : ", t_end
q_init = q_t[:, int(t_begin / cfg.IK_dt)] # after the predef takeoff
q_end = q_t[:, int(t_end / cfg.IK_dt)]
# compute limb-rrt path :
pathId = limb_rrt.generateLimbRRTPath(q_init, q_end, phase_previous, phase,
phase_next, fullBody)
if viewer and cfg.DISPLAY_FEET_TRAJ and DISPLAY_RRT:
from hpp.gepetto import PathPlayer
pp = PathPlayer(viewer)
if DISPLAY_JOINT_LEVEL:
pp.displayPath(pathId, jointName=fullBody.getLinkNames(eeName)[0])
else:
#TODO
pp.displayPath(
pathId,
jointName=fullBody.getLinkNames(eeName)[0],
offset=cfg.Robot.dict_offset[eeName].translation.T.tolist()[0])
# compute constraints for the end effector trajectories :
pData = bezier_com.ProblemData()
pData.c0_ = bezier_takeoff(bezier_takeoff.max())
pData.dc0_ = bezier_takeoff.derivate(bezier_takeoff.max(), 1)
pData.ddc0_ = bezier_takeoff.derivate(bezier_takeoff.max(), 2)
pData.j0_ = bezier_takeoff.derivate(bezier_takeoff.max(), 3)
pData.c1_ = bezier_landing(0)
pData.dc1_ = bezier_landing.derivate(0, 1)
pData.ddc1_ = bezier_landing.derivate(0, 2)
pData.j1_ = bezier_landing.derivate(0, 3)
pData.constraints_.flag_ = bezier_com.ConstraintFlag.INIT_POS | bezier_com.ConstraintFlag.INIT_VEL | bezier_com.ConstraintFlag.INIT_ACC | bezier_com.ConstraintFlag.END_ACC | bezier_com.ConstraintFlag.END_VEL | bezier_com.ConstraintFlag.END_POS | bezier_com.ConstraintFlag.INIT_JERK | bezier_com.ConstraintFlag.END_JERK
# now compute additional inequalities around the rrt path :
pDef = computeProblemConstraints(pData, fullBody, pathId, t_middle, eeName,
viewer)
solved = False
# loop and increase the number of variable control point until a solution is found
flagData = pData.constraints_.flag_
flags = [
bezier_com.ConstraintFlag.ONE_FREE_VAR, None,
bezier_com.ConstraintFlag.THREE_FREE_VAR, None,
bezier_com.ConstraintFlag.FIVE_FREE_VAR
]
numVars = 1
while not solved and numVars <= 5:
pData.constraints_.flag_ = flagData | flags[numVars - 1]
ineqEff = bezier_com.computeEndEffectorConstraints(pData, t_middle)
Hg = bezier_com.computeEndEffectorVelocityCost(pData, t_middle)
res = bezier_com.computeEndEffector(
pData, t_middle) #only used for comparison/debug ?
bezier_unconstrained = res.c_of_t
pDef.degree = bezier_unconstrained.degree
ineqConstraints = hpp_spline.generate_problem(pDef)
# _ prefix = previous notation (in bezier_com_traj)
# min x' H x + 2 g' x
# subject to A*x <= b
_A = np.vstack([ineqEff.A, ineqConstraints.A])
_b = np.vstack([ineqEff.b, ineqConstraints.b])
_H = Hg.A
_g = Hg.b
#_H = ineqConstraints.cost.A
#_g = ineqConstraints.cost.b
# quadprog notation :
#min (1/2)x' P x + q' x
#subject to G x <= h
#subject to C x = d
G = _A
h = _b.reshape((-1))
P = _H * 2.
q = (_g * 2.).flatten()
try:
if VERBOSE:
print "try to solve quadprog with " + str(
numVars) + " variable"
res = quadprog_solve_qp(P, q, G, h)
solved = True
except ValueError:
if VERBOSE:
print "Failed."
numVars += 2
if solved:
if VERBOSE:
print "Succeed."
else:
print "Constrained End effector Bezier method failed for all numbers of variables control points."
print "Return predef trajectory (may be in collision)."
return
# retrieve the result of quadprog and create a bezier curve :
vars = np.split(res, numVars)
wps = bezier_unconstrained.waypoints()
id_firstVar = 4
i = id_firstVar
for x in vars:
wps[:, i] = x
i += 1
bezier_middle = hpp_spline.bezier(wps, t_middle)
# create concatenation with takeoff/landing
curves = predef_curves.curves[::]
curves[id_middle] = bezier_middle
pBezier = PolyBezier(curves)
if VERBOSE:
print "time interval = ", time_interval[1] - time_interval[0]
print "polybezier length = ", pBezier.max()
ref_traj = trajectories.BezierTrajectory(pBezier, placement_init,
placement_end, time_interval)
return ref_traj
