def generatePredefBeziers(cfg, time_interval, placement_init, placement_end):
t_total = time_interval[1] - time_interval[0] - 2 * cfg.EFF_T_DELAY
logger.info("Generate Bezier Traj :")
logger.info("placement Init = %s", placement_init)
logger.info("placement End = %s", placement_end)
logger.info("time interval = %s", time_interval)
# generate two curves for the takeoff/landing :
t_takeoff_min = time_interval[0] + cfg.EFF_T_DELAY
t_takeoff_max = t_takeoff_min + cfg.EFF_T_PREDEF
t_landing_max = time_interval[1] - cfg.EFF_T_DELAY
t_landing_min = t_landing_max - cfg.EFF_T_PREDEF
bezier_takeoff = buildPredefinedInitTraj(cfg, placement_init, t_total,t_takeoff_min,t_takeoff_max)
bezier_landing = buildPredefinedFinalTraj(cfg, placement_end, t_total,t_landing_min,t_landing_max)
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. "
bezier_middle = generatePredefMiddle(bezier_takeoff, bezier_landing, t_takeoff_max,t_landing_min)
curves = piecewise_SE3()
# 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(bezier_takeoff.min()).reshape([-1,1]), time_interval[0], t_takeoff_min)
# Create SE3 curves with translation and duration defined from the bezier and constant orientation:
curves.append(SE3Curve(bezier_init_zero,placement_init.rotation, placement_init.rotation))
curves.append(SE3Curve(bezier_takeoff, placement_init.rotation, placement_init.rotation))
curves.append(SE3Curve(bezier_middle, placement_init.rotation, placement_end.rotation))
curves.append(SE3Curve(bezier_landing, placement_end.rotation, placement_end.rotation))
if cfg.EFF_T_DELAY > 0:
bezier_end_zero = bezier(bezier_landing(bezier_landing.max()).reshape([-1,1]), t_landing_max,time_interval[1])
# Create SE3 curves with translation and duration defined from the bezier and constant orientation:
curves.append(SE3Curve(bezier_end_zero,placement_end.rotation,placement_end.rotation))
return curves
def buildPredefinedInitTraj(cfg, placement, t_total,t_min,t_max):
p_off, v_off, a_off = computePredefConstants(cfg, t_total)
normal = placement.rotation @ np.array([0, 0, 1])
logger.debug("normal used for takeoff : %s",normal.T)
logger.debug("offset used : %s", p_off)
c0 = placement.translation.copy()
c1 = placement.translation.copy()
c1 += p_off * normal
logger.debug("takeoff part, c0 : %s",c0.T)
logger.debug("takeoff part, c1 : %s",c1.T)
dc0 = np.zeros(3)
#dc1 = v_off * normal
ddc0 = np.zeros(3)
#ddc1 = a_off * normal
#create wp :
n = 4.
wps = np.zeros(([3, int(n + 1)]))
T = t_max - t_min
# constrained init pos and final pos. Init vel, acc and jerk = 0
# c0
wps[:, 0] = (c0)
#dc0
wps[:, 1] = ((dc0 * T / n) + c0)
#ddc0 // * T because derivation make a T appear
wps[:, 2] = ((n * n * c0 - n * c0 + 2. * n * dc0 * T - 2. * dc0 * T + ddc0 * T * T) / (n * (n - 1.)))
#j0 = 0
wps[:, 3] = ((n * n * c0 - n * c0 + 3. * n * dc0 * T - 3. * dc0 * T + 3. * ddc0 * T * T) / (n * (n - 1.)))
#c1
wps[:, 4] = (c1)
return bezier(wps,t_min,t_max)
def test_conversion_curves(self):
print("test_conversion_curves")
__EPS = 1e-6
waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
a = bezier(waypoints)
# converting bezier to polynomial
a_pol = polynomial_from_bezier(a)
self.assertTrue(norm(a(0.3) - a_pol(0.3)) < __EPS)
# converting polynomial to hermite
a_chs = hermite_from_polynomial(a_pol)
self.assertTrue(norm(a_chs(0.3) - a_pol(0.3)) < __EPS)
# converting hermite to bezier
a_bc = bezier_from_hermite(a_chs)
self.assertTrue(norm(a_chs(0.3) - a_bc(0.3)) < __EPS)
self.assertTrue(norm(a(0.3) - a_bc(0.3)) < __EPS)
# converting bezier to hermite
a_chs = hermite_from_bezier(a)
self.assertTrue(norm(a(0.3) - a_chs(0.3)) < __EPS)
# converting hermite to polynomial
a_pol = polynomial_from_hermite(a_chs)
self.assertTrue(norm(a_pol(0.3) - a_chs(0.3)) < __EPS)
# converting polynomial to bezier
a_bc = bezier_from_polynomial(a_pol)
self.assertTrue(norm(a_bc(0.3) - a_pol(0.3)) < __EPS)
self.assertTrue(norm(a(0.3) - a_bc(0.3)) < __EPS)
return
def generatePredefMiddle(bezier_takeoff, bezier_landing, t_min,t_max):
T = t_max - t_min
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(bezier_landing.min())
dc1 = bezier_landing.derivate(bezier_landing.min(), 1)
ddc1 = bezier_landing.derivate(bezier_landing.min(), 2)
j1 = bezier_landing.derivate(bezier_landing.min(), 3)
n = 7
wps = np.zeros(([3, int(n + 1)]))
# c0
wps[:, 0] = (c0)
#dc0
wps[:, 1] = ((dc0 * T / n) + c0)
#ddc0 // * T because derivation make a T appear
wps[:, 2] = ((n * n * c0 - n * c0 + 2. * n * dc0 * T - 2. * dc0 * T + ddc0 * T * T) / (n * (n - 1.)))
#j0
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.)))
# j1
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)))
#ddc1 * T ??
wps[:, 5] = ((n * n * c1 - n * c1 - 2 * n * dc1 * T + 2 * dc1 * T + ddc1 * T * T) / (n * (n - 1)))
#dc1
wps[:, 6] = ((-dc1 * T / n) + c1)
#c1
wps[:, 7] = (c1)
return bezier(wps,t_min,t_max)
def setCOMfromCurve(phase, curve_normalized):
"""
Initialise the phase c_t dc_t and ddc_t from the given curve,
Also set the final values for the CoM from the final points of the curve
Also set or increase the final time from the duration of the curve
:param phase:
:param curve_normalized: the curve is defined in [0,max], we need to shift it
:return:
"""
if phase.c_t is None:
phase.c_t = piecewise()
phase.dc_t = piecewise()
phase.ddc_t = piecewise()
t_min = phase.timeInitial
# set initial values
phase.c_init = curve_normalized(0.)
phase.dc_init = curve_normalized.derivate(0., 1)
phase.ddc_init = curve_normalized.derivate(0., 2)
else:
t_min = phase.c_t.max()
t_max = t_min + curve_normalized.max()
# shift the curve to the correct initial time :
curve = bezier(curve_normalized.waypoints(), t_min, t_max)
# set the trajectory in the phase :
phase.c_t.append(curve)
phase.dc_t.append(curve.compute_derivate(1))
phase.ddc_t.append(curve.compute_derivate(2))
# set the new final values :
phase.timeFinal = t_max
phase.c_final = phase.c_t(t_max)
phase.dc_final = phase.dc_t(t_max)
phase.ddc_final = phase.ddc_t(t_max)
def buildPredefinedFinalTraj(cfg, placement, t_total,t_min,t_max):
p_off, v_off, a_off = computePredefConstants(cfg, t_total)
normal = placement.rotation @ np.array([0, 0, 1])
logger.debug("normal used for landing : %s", normal.T)
logger.debug("offset used : %s", p_off)
c0 = placement.translation.copy()
c1 = placement.translation.copy()
c0 += p_off * normal
logger.debug("landing part, c0 : %s", c0.T)
logger.debug("landing part, c1 : %s", c1.T)
dc1 = np.zeros(3)
#dc0 = v_off * normal
ddc1 = np.zeros(3)
#ddc0 = a_off * normal
#create wp :
n = 4.
wps = np.zeros(([3, int(n + 1)]))
T = t_max - t_max
# constrained init pos and final pos. final vel, acc and jerk = 0
#c0
wps[:, 0] = (c0)
# j1
wps[:, 1] = ((n * n * c1 - n * c1 - 3 * n * dc1 * T + 3 * dc1 * T + 3 * ddc1 * T * T) / (n * (n - 1)))
#ddc1 * T ??
wps[:, 2] = ((n * n * c1 - n * c1 - 2 * n * dc1 * T + 2 * dc1 * T + ddc1 * T * T) / (n * (n - 1)))
#dc1
wps[:, 3] = ((-dc1 * T / n) + c1)
#c1
wps[:, 4] = (c1)
return bezier(wps, t_min, t_max)
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(cfg, time_interval, placement_init, placement_end):
t_tot = time_interval[1] - time_interval[0]
wps = 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()
translation = bezier(wps, time_interval[0], time_interval[1])
pBezier = piecewise_SE3(SE3Curve(translation, placement_init.rotation, placement_end.rotation))
return pBezier
def test_piecewise_bezier3_curve(self):
# To test :
# - Functions : constructor, min, max, derivate, add_curve, is_continuous
waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
a = bezier(waypoints, 0., 1.)
b = bezier(waypoints, 1., 2.)
pc = piecewise_bezier_curve(a)
pc.add_curve(b)
pc.min()
pc.max()
pc(0.4)
self.assertTrue((pc(pc.min()) == matrix([1., 2.,
3.]).transpose()).all())
self.assertTrue((pc.derivate(0.4, 0) == pc(0.4)).all())
pc.derivate(0.4, 2)
pc.is_continuous(0)
pc.is_continuous(1)
# Test serialization
pc.saveAsText("serialization_pc.test")
pc_test = piecewise_bezier_curve()
pc_test.loadFromText("serialization_pc.test")
self.assertTrue((pc(0.4) == pc_test(0.4)).all())
os.remove("serialization_pc.test")
return
def __check_trajectory(p0,
p1,
p2,
p3,
T,
H,
mass,
g,
time_step=0.1,
dL=allZeros):
if time_step < 0:
time_step = 0.01
resolution = int(T / time_step)
wps = [p0, p1, p2, p3]
wps = matrix([pi.tolist() for pi in wps]).transpose()
c_t = bezier(wps)
ddc_t = c_t.compute_derivate(2)
def c_tT(t):
return asarray(c_t(t / T)).flatten()
def ddc_tT(t):
return 1. / (T * T) * asarray(ddc_t(t / T)).flatten()
def dL_tT(t):
return 1. / (T) * asarray(dL(t / T)).flatten()
for i in range(resolution):
t = T * float(i) / float(resolution)
if not (is_stable(H,
c=c_tT(t),
ddc=ddc_tT(t),
dL=dL_tT(t),
m=mass,
g_vec=g,
robustness=-0.00001)):
if t > 0.1:
raise ValueError("trajectory is not stale ! at ", t)
else:
print(
is_stable(H,
c=c_tT(t),
ddc=ddc_tT(t),
dL=asarray(dL(t / T)).flatten(),
m=mass,
g_vec=g,
robustness=-0.00001))
print("failed at 0")
def MakeSpline2D(in_LB):
print('draw spline ')
# #We describe a degree 3 curve as a Bezier curve with 4 control points
distance = in_LB.point_x[-1] - in_LB.point_x[0]
distance_x = abs(distance)
distance = in_LB.point_y[-1] - in_LB.point_y[0]
distance_y = abs(distance)
waypoints = array([[in_LB.point_x[0], in_LB.point_y[0], 0],
[in_LB.point_x[0] + distance_x, in_LB.point_y[0], 0],
[in_LB.point_x[0], in_LB.point_y[0] + distance_y, 0],
[in_LB.point_x[-1], in_LB.point_y[-1], 0]]).transpose()
ref = bezier(waypoints)
plotBezier2D(ref, ax=ax_mouse, showControlPoints=True)
def generateSmoothBezierTrajWithPredef(cfg, time_interval, placement_init, placement_end):
predef_curves = generatePredefBeziers(cfg, time_interval, placement_init, placement_end)
id_middle = int(math.floor(predef_curves.num_curves() / 2.))
bezier_takeoff = predef_curves.curve_at_index(id_middle-1).translation_curve()
bezier_landing = predef_curves.curve_at_index(id_middle+1).translation_curve()
# update mid curve to minimize velocity along the curve:
# set problem data for mid curve :
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(bezier_landing.min())
pData.dc1_ = bezier_landing.derivate(bezier_landing.min(), 1)
pData.ddc1_ = bezier_landing.derivate(bezier_landing.min(), 2)
pData.j1_ = bezier_landing.derivate(bezier_landing.min(), 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
t_min_middle = predef_curves.curve_at_index(id_middle).min()
t_max_middle = predef_curves.curve_at_index(id_middle).max()
t_middle = t_max_middle - t_min_middle
res = bezier_com.computeEndEffector(pData, t_middle)
wp_middle = res.c_of_t.waypoints()
bezier_middle = bezier(wp_middle,t_min_middle,t_max_middle)
# create a new piecewise-bezier, with the predef curves except for bezier_middle :
pBezier = piecewise_SE3()
for i in range(predef_curves.num_curves()):
if i == id_middle:
pBezier.append(SE3Curve(bezier_middle, placement_init.rotation, placement_end.rotation))
else:
pBezier.append(predef_curves.curve_at_index(i))
return pBezier
def MakeBeizerConvexhull2D(in_LB):
#dimension of our problem (here 3 as our curve is 3D)
dim = 2 # dimension = 3, 3D
refDegree = 3 # degree = 3, polynomial we have to get 4 control points (start,end,c1,c2)
pD = problem_definition(dim)
pD.degree = refDegree #we want to fit a curve of the same degree as the reference curve for the sanity check
pD.flag = constraint_flag.INIT_POS | constraint_flag.END_POS
#set initial position
pD.init_pos = array([in_LB.point_x[0], in_LB.point_y[0]])
#set end position
pD.end_pos = array(
[in_LB.point_x[-1], in_LB.point_y[-1]]
) # -1 refers to the last index, -2 refers to the second last index and so on
problem = setup_control_points(pD)
#generates the variable bezier curve with the parameters of problemDefinition
variableBezier = problem.bezier()
#We describe a degree 3 curve as a Bezier curve with 4 control points
distance = in_LB.point_x[-1] - in_LB.point_x[0]
distance_x = abs(distance)
distance = in_LB.point_y[-1] - in_LB.point_y[0]
distance_y = abs(distance)
waypoints = array([
[in_LB.point_x[0], in_LB.point_y[0]],
[in_LB.point_x[0] + distance_x, in_LB.point_y[0]],
[in_LB.point_x[0], in_LB.point_y[0] + distance_y],
[in_LB.point_x[-1], in_LB.point_y[-1]]
]).transpose() #[in_LB.point_x[0],in_LB.point_y[0]+distance_y],
ref = bezier(waypoints)
#plotting sampled points
numSamples = 10
fNumSamples = float(numSamples)
ptsTime = [(ref(float(t) / fNumSamples), float(t) / fNumSamples)
for t in range(numSamples + 1)]
#least square form of ||Ax-b||**2
def to_least_square(A, b):
return dot(A.T, A), -dot(A.T, b)
def genCost(variableBezier, ptsTime):
#first evaluate variableBezier for each time sampled
allsEvals = [(variableBezier(time), pt) for (pt, time) in ptsTime]
#then compute the least square form of the cost for each points
allLeastSquares = [
to_least_square(el.B(),
el.c() + pt) for (el, pt) in allsEvals
] # el.c() + pt ?
#and finally sum the costs
Ab = [sum(x) for x in zip(*allLeastSquares)]
return Ab[0], Ab[1]
A, b = genCost(variableBezier, ptsTime)
costAb = generate_integral_problem(pD, integral_cost_flag.ACCELERATION)
problemSize = problem.numVariables * dim # what is this? dimension * degree ?
convexhull_1_ineq = array(in_LB.convexhull1.equations)
convexhull_2_ineq = array(in_LB.convexhull2.equations)
nConvexEq = convexhull_1_ineq.shape[0]
ineqMatrix = zeros((nConvexEq * 4 + nConvexEq * 4 * 2 + nConvexEq * 4,
problemSize)) # C1,C2[4*2*2]
ineqVector = zeros(nConvexEq * 4 + nConvexEq * 4 * 2 + nConvexEq * 4)
# ineqMatrix = zeros((convexhull_1_ineq.shape[0] * 4 * 2, problemSize)) # C1,C2[4*2*2]
# ineqVector = zeros(convexhull_1_ineq.shape[0] * 4 * 2)
array_CB = np.array([], dtype=float).reshape(0, 4)
vec_CB = []
piecewiseCurve = variableBezier.split(array([[0.3, 0.6]]).T)
# left side
constrainedCurve = piecewiseCurve.curve_at_index(0)
array_C_cur = array(convexhull_1_ineq)[:, 0:2]
vec_C_cur = -array(convexhull_1_ineq)[:, 2]
for k in range(4):
mat_Info_Point_i = constrainedCurve.waypointAtIndex(k)
vec_C_temp = vec_C_cur - array_C_cur.dot(mat_Info_Point_i.c())
arr_C_temp = dot(array_C_cur, mat_Info_Point_i.B())
array_CB = np.vstack([array_CB, arr_C_temp])
vec_CB = np.hstack([vec_CB, vec_C_temp])
# overlapped space
constrainedCurve = piecewiseCurve.curve_at_index(1)
for i in range(2):
if i == 0:
array_C_cur = array(convexhull_1_ineq)[:, 0:2]
vec_C_cur = -array(convexhull_1_ineq)[:, 2]
else:
array_C_cur = array(convexhull_2_ineq)[:, 0:2]
vec_C_cur = -array(convexhull_2_ineq)[:, 2]
for k in range(4):
print array_C_cur
mat_Info_Point_i = constrainedCurve.waypointAtIndex(k)
vec_C_temp = vec_C_cur - array_C_cur.dot(mat_Info_Point_i.c())
arr_C_temp = dot(array_C_cur, mat_Info_Point_i.B())
array_CB = np.vstack([array_CB, arr_C_temp])
vec_CB = np.hstack([vec_CB, vec_C_temp])
# right side
constrainedCurve = piecewiseCurve.curve_at_index(2)
array_C_cur = array(convexhull_2_ineq)[:, 0:2]
vec_C_cur = -array(convexhull_2_ineq)[:, 2]
for k in range(4):
mat_Info_Point_i = constrainedCurve.waypointAtIndex(k)
vec_C_temp = vec_C_cur - array_C_cur.dot(mat_Info_Point_i.c())
arr_C_temp = dot(array_C_cur, mat_Info_Point_i.B())
array_CB = np.vstack([array_CB, arr_C_temp])
vec_CB = np.hstack([vec_CB, vec_C_temp])
ineqMatrix = array_CB
ineqVector = vec_CB
res = quadprog_solve_qp(costAb.cost.A,
costAb.cost.b,
G=ineqMatrix,
h=ineqVector)
fitBezier = variableBezier.evaluate(res.reshape((-1, 1)))
#now plotting the obtained curve, in red the concerned part
piecewiseFit = fitBezier.split(array([[0.3, 0.6]]).T)
plotBezier2D(piecewiseFit.curve_at_index(0), ax=ax_mouse, color="b")
plotBezier2D(piecewiseFit.curve_at_index(1), ax=ax_mouse, color="r")
plotBezier2D(piecewiseFit.curve_at_index(2), ax=ax_mouse, color="b")
plt.draw()
P1_F = [xF0 - 0.125 * L, z0]
P2_F = [xF0 - 0.5 * L, z0 + 0.8 * h]
P3_F = [xF0 - 0.5 * L, z0 + 0.8 * h]
P4_F = [xF0 - 0.5 * L, z0 + 0.8 * h]
P5_F = [xF0 + 0.5 * L, z0 + 0.8 * h]
P6_F = [xF0 + 0.5 * L, z0 + 0.8 * h]
P7_F = [xF0 + 0.5 * L, z0 + h]
P8_F = [xF0 + 1.5 * L, z0 + h]
P9_F = [xF0 + 1.5 * L, z0 + h]
P10_F = [xF0 + 1.125 * L, z0]
P11_F = [xF0 + L, z0]
waypoints_F = np.matrix(
[P0_F, P1_F, P2_F, P3_F, P4_F, P5_F, P6_F, P7_F, P8_F, P9_F, P10_F,
P11_F]).T
bc_F = bezier(waypoints_F, 0.0, Tsw)
def ftraj_Front(t): #arguments : time
x = []
z = []
t %= T
if t <= Tsw:
x.append(bc_F(t)[0, 0])
z.append(bc_F(t)[1, 0])
else:
t -= Tsw
bezierLinear = problem.bezier()
allsEvals = [(bezierLinear(time), pt) for (pt, time) in ptsTime]
allLeastSquares = [to_least_square(el.B(), el.c() + pt) for (el, pt) in allsEvals]
Ab = [sum(x) for x in zip(*allLeastSquares)]
res = quadprog_solve_qp(Ab[0] + identity(problem.numVariables * dim) * 0.0001, Ab[1])
return bezierLinear.evaluate(res.reshape((-1, 1)))
# return A, b
if __name__ == '__main__':
waypoints = array([[1., 2., 3., 0.], [-4., -5., -6., 0.], [4., 5., 6., 0.], [7., 8., 9., 0.]]).transpose()
a = bezier(waypoints)
totalTime = 1.
degree = a.nbWaypoints - 1
ptsTime = [(a(float(i) / 10.), float(i) / 10.) for i in range(11)]
dim = waypoints.shape[0]
c = curve_constraints(dim)
c.init_vel = array([[0., 0., 0., 0.]]).T
def plot_fit_and_original(ax, plotControlPoints=False):
plotBezier(a, ax=ax)
plotBezier(bFit, ax=ax, color="g")
if plotControlPoints:
plotControlPoints(a, ax=ax)
plotControlPoints(bFit, ax=ax, color="g")
import crocoddyl
import curves
import eigenpy
import example_robot_data
import hppfcl
import multicontact_api
import numpy as np
import pinocchio
import tsid
with open("/dist") as f:
dist = f.read()
if "20.04" in dist:
print("*" * 74)
print("{: <6s}".format(sys.version.split()[0]))
print(eigenpy.Quaternion(1, 2, 3, 4).norm())
print(hppfcl.Capsule(2, 3).computeVolume())
print(pinocchio.SE3.Identity().inverse())
print(example_robot_data.load("talos").model.nq)
URDF = "/talos_data/robots/talos_left_arm.urdf"
PATH = example_robot_data.robots_loader.getModelPath(URDF)
print(tsid.RobotWrapper(PATH + URDF, [PATH], False).na)
print(crocoddyl.ActionModelUnicycle().nr)
print(
curves.bezier(
np.array([[1, 2, 3], [4, 5, 6], [4, 5, 6], [4, 5, 6], [4, 5, 6]]), 0.2,
1.5).dim())
print(multicontact_api.ContactModel().mu)
def generate_effector_trajectory_limb_rrt_optimized(cfg,
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(cfg, time_interval, placement_init,
placement_end)
else:
if q_t is None or phase_previous is None or phase is None or phase_next is None or not fullBody or not eeName:
raise ValueError(
"Cannot compute LimbRRTOptimizedTraj for try >= 1 without optionnal arguments"
)
if cfg.EFF_T_PREDEF > 0:
predef_curves = generatePredefBeziers(cfg, time_interval,
placement_init, placement_end)
else:
predef_curves = generateSmoothBezierTraj(cfg, time_interval,
placement_init, placement_end)
id_middle = int(math.floor(predef_curves.num_curves() / 2.))
predef_middle = predef_curves.curve_at_index(id_middle).translation_curve()
pos_init = predef_middle(predef_middle.min())
pos_end = predef_middle(predef_middle.max())
logger.warning("generateLimbRRTOptimizedTraj, try number %d", numTry)
logger.info("bezier takeoff end : %s", pos_init)
logger.info("bezier landing init : %s", pos_end)
t_begin = predef_middle.min()
t_end = predef_middle.max()
t_middle = t_end - t_begin
logger.info("t begin : %f", t_begin)
logger.info("t end : %f", t_end)
q_init = q_t(t_begin)
q_end = q_t(t_end)
global current_limbRRT_id
# compute new limb-rrt path if needed:
if not current_limbRRT_id or (numTry in recompute_rrt_at_tries):
logger.warning("Compute new limb-rrt path ...")
current_limbRRT_id = generateLimbRRTPath(q_init, q_end, phase_previous,
phase, phase_next, fullBody)
if viewer and cfg.DISPLAY_FEET_TRAJ and DISPLAY_RRT_PATH:
from hpp.gepetto import PathPlayer
pp = PathPlayer(viewer)
pp.displayPath(current_limbRRT_id,
jointName=fullBody.getLinkNames(eeName)[0])
# find weight and number of variable to use from the numTry :
for offset in reversed(recompute_rrt_at_tries):
if numTry >= offset:
id = numTry - offset
break
logger.info("weights_var id = %d", id)
if id >= len(weights_vars):
raise ValueError(
"Max number of try allow to find a collision-end effector trajectory reached."
)
weight = weights_vars[id][0]
varFlag = weights_vars[id][1]
numVars = weights_vars[id][2]
logger.warning("use weight %f with num free var = %d", weight, numVars)
# compute constraints for the end effector trajectories :
pData = bezier_com.ProblemData()
pData.c0_ = predef_middle(predef_middle.min())
pData.dc0_ = predef_middle.derivate(predef_middle.min(), 1)
pData.ddc0_ = predef_middle.derivate(predef_middle.min(), 2)
pData.j0_ = predef_middle.derivate(predef_middle.min(), 3)
pData.c1_ = predef_middle(predef_middle.max())
pData.dc1_ = predef_middle.derivate(predef_middle.max(), 1)
pData.ddc1_ = predef_middle.derivate(predef_middle.max(), 2)
pData.j1_ = predef_middle.derivate(predef_middle.max(), 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 \
| varFlag
Constraints = bezier_com.computeEndEffectorConstraints(pData, t_middle)
Cost_smooth = bezier_com.computeEndEffectorVelocityCost(pData, t_middle)
Cost_distance = computeDistanceCostMatrices(fullBody, current_limbRRT_id,
pData, t_middle, eeName)
# formulate QP matrices :
# _ prefix = previous notation (in bezier_com_traj)
# min x' H x + 2 g' x
# subject to A*x <= b
_A = Constraints.A
_b = Constraints.b
_H = ((1. - weight) * Cost_smooth.A + weight * Cost_distance.A)
_g = ((1. - weight) * Cost_smooth.b + weight * Cost_distance.b)
logger.debug("A = %s", _A)
logger.debug("b = %s", _b)
logger.debug("H = %s", _H)
logger.debug("h = %s", _g)
"""
_A = np.array(_A)
_b = np.array(_b)
_H = np.array(_H)
_g = np.array(_g)
"""
# quadprog notation :
#min (1/2)x' P x + q' x
#subject to G x <= h
#subject to C x = d
G = _A
h = _b.flatten() # remove the transpose when working with array
P = _H * 2.
q = (_g * 2.).flatten()
logger.debug("G = %s", G)
logger.debug("h = %s", h)
logger.debug("P = %s", P)
logger.debug("q = %s", q)
logger.debug("Shapes : ")
logger.debug("G : %s", G.shape)
logger.debug("h : %s", h.shape)
logger.debug("P : %s", P.shape)
logger.debug("q : %s", q.shape)
# solve the QP :
solved = False
try:
res = quadprog_solve_qp(P, q, G, h)
solved = True
except ValueError as e:
logger.error("Quadprog error : ", exc_info=e)
raise ValueError(
"Quadprog failed to solve QP for optimized limb-RRT end-effector trajectory, for try number "
+ str(numTry))
logger.info("Quadprog solved.")
# 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
logger.debug("Constant waypoints computed.")
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] = np.array(x)
logger.debug("waypoint number %d : %s", i, wps[:, i])
i += 1
logger.debug("Variables waypoints replaced by quadprog results.")
bezier_middle = bezier(wps, t_begin, t_end)
# create concatenation with takeoff/landing
pBezier = piecewise_SE3()
for ci in range(predef_curves.num_curves()):
if ci == id_middle:
pBezier.append(
SE3Curve(bezier_middle, placement_init.rotation,
placement_end.rotation))
else:
pBezier.append(predef_curves.curve_at_index(ci))
logger.info("time interval = [%f ; %f]", pBezier.min(), pBezier.max())
return pBezier
def test_bezier(self):
print("test_bezier")
# To test :
# - Functions : constructor, min, max, derivate,compute_derivate, compute_primitive
# - Variables : degree, nbWayPoints
__EPS = 1e-6
waypoints = matrix([[1., 2., 3.]]).T
a = bezier(waypoints, 0., 2.)
t = 0.
while t < 2.:
self.assertTrue(norm(a(t) - matrix([1., 2., 3.]).T) < __EPS)
t += 0.1
waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
# time_waypoints = matrix([0., 1.]).transpose()
# Create bezier6 and bezier
a = bezier(waypoints, 0., 3.)
# Test waypoints
self.assertTrue(a.nbWaypoints == 2)
for i in range(0, a.nbWaypoints):
if i == 0:
self.assertTrue(
(a.waypointAtIndex(0) == matrix([1., 2.,
3.]).transpose()).all())
elif i == 1:
self.assertTrue(
(a.waypointAtIndex(1) == matrix([4., 5.,
6.]).transpose()).all())
# self.assertTrue((a.waypoints == waypoints).all())
# Test : Degree, min, max, derivate
# self.print_str(("test 1")
self.assertEqual(a.degree, a.nbWaypoints - 1)
a.min()
a.max()
a(0.4)
self.assertTrue((a(a.min()) == matrix([1., 2., 3.]).transpose()).all())
self.assertTrue((a.derivate(0.4, 0) == a(0.4)).all())
a.derivate(0.4, 2)
a = a.compute_derivate(100)
prim = a.compute_primitive(1)
# Check primitive and derivate - order 1
for i in range(10):
t = float(i) / 10.
self.assertTrue((a(t) == prim.derivate(t, 1)).all())
self.assertTrue((prim(0) == matrix([0., 0., 0.])).all())
# Check primitive and derivate - order 2
prim = a.compute_primitive(2)
for i in range(10):
t = float(i) / 10.
self.assertTrue((a(t) == prim.derivate(t, 2)).all())
self.assertTrue((prim(0) == matrix([0., 0., 0.])).all())
# Create new bezier curve
waypoints = matrix([[1., 2., 3.], [4., 5., 6.], [4., 5., 6.],
[4., 5., 6.], [4., 5., 6.]]).transpose()
a0 = bezier(waypoints)
a1 = bezier(waypoints, 0., 3.)
prim0 = a0.compute_primitive(1)
prim1 = a1.compute_primitive(1)
# Check change in argument time_t of bezier
for i in range(10):
t = float(i) / 10.
self.assertTrue(norm(a0(t) - a1(3 * t)) < __EPS)
self.assertTrue(
norm(a0.derivate(t, 1) - a1.derivate(3 * t, 1) * 3.) < __EPS)
self.assertTrue(
norm(a0.derivate(t, 2) - a1.derivate(3 * t, 2) * 9.) < __EPS)
self.assertTrue(norm(prim0(t) - prim1(t * 3) / 3.) < __EPS)
self.assertTrue((prim(0) == matrix([0., 0., 0.])).all())
# testing bezier with constraints
c = curve_constraints()
c.init_vel = matrix([0., 1., 1.]).transpose()
c.end_vel = matrix([0., 1., 1.]).transpose()
c.init_acc = matrix([0., 1., -1.]).transpose()
c.end_acc = matrix([0., 100., 1.]).transpose()
# Check derivate with constraints
waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
a = bezier(waypoints, c)
self.assertTrue(norm(a.derivate(0, 1) - c.init_vel) < 1e-10)
self.assertTrue(norm(a.derivate(1, 2) - c.end_acc) < 1e-10)
# Test serialization : bezier 3
a.saveAsText("serialization_curve.test")
# waypoints = matrix([[0,0,0,], [0,0,0,]]).transpose()
b = bezier()
b.loadFromText("serialization_curve.test")
self.assertTrue((a(0.4) == b(0.4)).all())
os.remove("serialization_curve.test")
# Bezier dim 4
waypoints = matrix([[1., 2., 3., 4.]]).T
a = bezier(waypoints, 0., 2.)
# Test serialization : bezier of dim 4
a.saveAsText("serialization_curve.test")
# waypoints = matrix([[0,0,0,], [0,0,0,]]).transpose()
b = bezier()
b.loadFromText("serialization_curve.test")
self.assertTrue((a(0.4) == b(0.4)).all())
os.remove("serialization_curve.test")
return
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.tolist())
# 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 = curves.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 = curves.bezier(wps, 0., 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 toBezier3(self, x):
wps = []
for i in range(self.bezier.nbWaypoints):
mat, vec = self.matrixFromWaypoints(i)
wps += [mat.dot(x) + vec]
return bezier(array(wps).transpose(), self.bezier.max())
def test_continuous_cpp_vs_continuous_py(N_CONTACTS=2,
solver='qpoases',
verb=0):
mu = 0.5
# friction coefficient
lx = 0.1
# half foot size in x direction
ly = 0.07
# half foot size in y direction
# First, generate a contact configuration
CONTACT_POINT_UPPER_BOUNDS = [0.5, 0.5, 0.5]
CONTACT_POINT_LOWER_BOUNDS = [-0.5, -0.5, 0.0]
gamma = atan(mu)
# half friction cone angle
RPY_LOWER_BOUNDS = [-2 * gamma, -2 * gamma, -pi]
RPY_UPPER_BOUNDS = [+2 * gamma, +2 * gamma, +pi]
MIN_CONTACT_DISTANCE = 0.3
global mass
global g_vector
X_MARG = 0.07
Y_MARG = 0.07
succeeded = False
while (not succeeded):
p, N = generate_contacts(N_CONTACTS, lx, ly, mu,
CONTACT_POINT_LOWER_BOUNDS,
CONTACT_POINT_UPPER_BOUNDS, RPY_LOWER_BOUNDS,
RPY_UPPER_BOUNDS, MIN_CONTACT_DISTANCE, False)
X_LB = np.min(p[:, 0] - X_MARG)
X_UB = np.max(p[:, 0] + X_MARG)
Y_LB = np.min(p[:, 1] - Y_MARG)
Y_UB = np.max(p[:, 1] + Y_MARG)
Z_LB = np.max(p[:, 2] + 0.3)
Z_UB = np.max(p[:, 2] + 1.5)
# (H,h) = compute_GIWC(p, N, mu, False);
H = -compute_CWC(p, N, mass, mu)
h = zeros(H.shape[0])
(succeeded, c0) = find_static_equilibrium_com(mass, [X_LB, Y_LB, Z_LB],
[X_UB, Y_UB, Z_UB], H, h)
dc0 = np.random.uniform(-1, 1, size=3)
Z_MIN = np.max(p[:, 2]) - 0.1
Ineq_kin = zeros([3, 3])
Ineq_kin[2, 2] = -1
ineq_kin = zeros(3)
ineq_kin[2] = -Z_MIN
bezierSolver = BezierZeroStepCapturability(
"ss",
c0,
dc0,
p,
N,
mu,
g_vector,
mass,
verb=verb,
solver=solver,
kinematic_constraints=[Ineq_kin, ineq_kin])
eqCpp = Equilibrium("dyn_eq2", mass, 4)
eqCpp.setNewContacts(asmatrix(p), asmatrix(N), mu,
EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP)
# bezierSolver = BezierZeroStepCapturability("ss", c0, dc0, p, N, mu, g_vector, mass, verb=verb, solver=solver,
# kinematic_constraints = None);
# stabilitySolver = StabilityCriterion("ss", c0, dc0, p, N, mu, g_vector, mass, verb=verb, solver=solver);
window_times = [1] + [0.1 * i for i in range(1, 10)] + [
0.1 * i for i in range(11, 21)
] # try nominal time first
# window_times = [0.2*i for i in range(1,5)] + [0.2*i for i in range(6,11)] #try nominal time first
# window_times = [1]+ [0.4*i for i in range(1,4)] #try nominal time first
# window_times = [1]+ [0.4*i for i in range(3,6)] #try nominal time first
# window_times = [0.7]
found = False
time_step_check = -0.2
for i, el in enumerate(window_times):
if (found):
break
res = bezierSolver.can_I_stop(T=el, time_step=time_step_check)
if (res.is_stable):
found = True
# print("continuous found at ", el)
__check_trajectory(bezierSolver._p0,
bezierSolver._p1,
res.c,
res.c,
el,
bezierSolver._H,
bezierSolver._mass,
bezierSolver._g,
time_step=time_step_check,
dL=bezier(
matrix([p_i.tolist()
for p_i in res.wpsdL]).transpose()))
if i != 0:
print(
"continuous Failed to stop at 1, but managed to stop at ",
el)
found = False
time_step_check = 0.05
for i, el in enumerate(window_times):
if (found):
break
res2 = bezierSolver.can_I_stop(T=el,
time_step=time_step_check,
l0=zeros(3))
if (res2.is_stable):
found = True
# print("ang_momentum found at ", el)
__check_trajectory(
bezierSolver._p0,
bezierSolver._p1,
res2.c,
res2.c,
el,
bezierSolver._H,
# bezierSolver._mass, bezierSolver._g, time_step = time_step_check, dL = res2.dL_of_t)
bezierSolver._mass,
bezierSolver._g,
time_step=time_step_check,
dL=bezier(
matrix([p_i.tolist() for p_i in res2.wpsdL]).transpose()))
if i != 0:
print(
"ang_momentum Failed to stop at 1, but managed to stop at ",
el)
# res2 = None
# try:
# res2 = stabilitySolver.can_I_stop();
# except Exception as e:
# pass
if (res2.is_stable != res.is_stable):
if (res.is_stable):
print("continuous won")
else:
print("ang_momentum won")
return res2.is_stable, res.is_stable, res2, res, c0, dc0, H, h, p, N
