def _gen_approx(f, n, constraints): nf = float(n) waypoints = matrix([f(float(i) / nf) for i in range(n+1)]).transpose() if constraints == None: return bezier(waypoints) else: return bezier(waypoints, constraints)
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 __Bezier(wps, init_acc = [0.,0.,0.], end_acc = [0.,0.,0.], init_vel = [0.,0.,0.], end_vel = [0.,0.,0.]):
c = curve_constraints();
c.init_vel = matrix(init_vel);
c.end_vel = matrix(end_vel);
c.init_acc = matrix(init_acc);
c.end_acc = matrix(end_acc);
matrix_bezier = matrix(wps).transpose()
curve = bezier(matrix_bezier, c)
return __curveToWps(curve), curve
def bezier_traj(cs_ddcs, init_dc_ddc = (zero3,zero3), end_dc_ddc = (zero3,zero3)): assert len(cs_ddcs) >= 2, "cannot create com trajectory with less than 2 points" #creating waypoints for curve waypoints = matrix([ c for (c,_) in cs_ddcs]).transpose() c = curve_constraints(); c.init_vel = matrix(init_dc_ddc[0]); c.end_vel = matrix(end_dc_ddc[0]); c.init_acc = matrix(init_dc_ddc[1]); c.end_acc = matrix(end_dc_ddc[1]); c_t = bezier(waypoints, c) return __compute_c_ddc_t(c_t), c_t
def bezier_traj(cs_ddcs, m=54., g_vec=array([0., 0., -9.81]), mu=0.6):
assert len(
cs_ddcs) >= 2, "cannot create com trajectory with less than 2 points"
#creating waypoints for curve
waypoints = matrix([
compute_w(c, ddc, m=m, g_vec=g_vec) for (c, ddc) in cs_ddcs
]).transpose()
print "waypoints", waypoints
w1_t = bezier(waypoints)
#~ return w1_t
#~ return __compute_c_t(w1_t, cs_ddcs[0][0], g_vec)
return __compute_c_ddc_t(w1_t, cs_ddcs[0][0], cs_ddcs[-1][0], m, g_vec)
return w1_t
def __compute_c_t(b_curve, c_0, c_end, g_vec):
wc_t = b_curve.compute_primitive(2)
delta_ = c_0 - wc_t(0.)[0:3, :] #make sure c_0 is reached
c_end_offset = c_end - g_vec / 2. - delta_
wps = wc_t.waypoints()
wps[0:3, -1] = c_end_offset
wc_t = bezier(wps)
def c_of_t(t):
print "wc_t, ", wc_t(0.)[0:3, :]
print "c_0, ", c_0
print "delta, ", delta_
print "wc_t(t), ", wc_t(t)[0:3, :]
print "g_vec, ", g_vec
return asarray(delta_ + wc_t(t)[0:3, :] +
0.5 * g_vec * t * t).flatten()
return c_of_t, wc_t
return v.reshape([-1, 1])
def m2v(m):
return v.flatten()
def m2l(m):
return m.flatten().tolist()
from spline import bezier
import time
waypoints = array([[1., 2., 3.], [4., 5., 6.]]).transpose()
a = bezier(waypoints, 3.)
def update():
print project(hyq)
q_ik = getConfig(hyq)
#~ setConfig(hyq, q_ik)
q = m2l(q_ik)
r(q)
clearProjector(hyq)
return q
def followtraj(ikHelper, fram_traj_list, maintain_constraints):
init_clock = time.clock()
timer = 0
from spline import bezier, bezier6, polynom, exact_cubic, curve_constraints, spline_deriv_constraint from numpy import matrix from numpy.linalg import norm waypoints = matrix([[1.,2.,3.],[4.,5.,6.]]).transpose() waypoints6 = matrix([[1.,2.,3.,7.,5.,5.],[4.,5.,6.,4.,5.,6.]]).transpose() time_waypoints = matrix([0.,1.]) #testing bezier curve a = bezier6(waypoints6) a = bezier(waypoints, -1., 3.) assert(a.degree == a.nbWaypoints -1) a.min() a.max() a(0.4) assert((a.derivate(0.4,0) == a(0.4)).all()) 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(a(t) == prim.derivate(t,1)).all() assert(prim(0) == matrix([0.,0.,0.])).all() prim = a.compute_primitive(2) for i in range(10): t = float(i) / 10.
def test_continuous_cpp_vs_continuous_py(N_CONTACTS = 2, solver='qpoases', verb=0):
DO_PLOTS = False;
PLOT_3D = False;
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
def can_I_stop(self,
c0=None,
dc0=None,
T=1.,
MAX_ITER=None,
time_step=-1,
l0=None,
asLp=False):
''' Determine whether the system can come to a stop without changing contacts.
Keyword arguments:
c0 -- initial CoM position
dc0 -- initial CoM velocity
T -- the EXACT given time to stop
time_step -- if negative, a continuous resolution is used
to guarantee that the trajectory is feasible. If > 0, then
used a discretized approach to validate trajectory. This allows
to have control points outside the cone, which is supposed to increase the
solution space.
l0 : if equals None, angular momentum is not considered and set to 0. Else
it becomes a variable of the problem and l0 is the initial angular momentum
asLp : If true, problem is solved as an LP. If false, solved as a qp that
minimizes distance to original point (weight of 0.001) and angular momentum if applies (weight of 1.)
Output: An object containing the following member variables:
is_stable -- boolean value
c -- final com position
dc -- final com velocity. [WARNING] if is_stable is False, not used
ddc_min -- [WARNING] Not relevant (used)
t -- always T (Bezier curve)
computation_time -- time taken to solve all the LPs
c_of_t, dc_of_t, ddc_of_t: trajectories and derivatives in function of the time
dL_of_t : trajectory of the angular momentum along time
wps : waypoints of the solution bezier curve c*(s)
wpsL : waypoints of the solution angular momentum curve L*(s) Zero if no angular mementum
wpsdL : waypoints of the solution angular momentum curve dL*(s) Zero if no angular mementum
'''
if T <= 0.:
raise ValueError('T cannot be lesser than 0')
print "\n *** [WARNING] In bezier step capturability: you set a T_0 or MAX_ITER value, but they are not used by the algorithm"
if MAX_ITER != None:
print "\n *** [WARNING] In bezier step capturability: you set a T_0 or MAX_ITER value, but they are not used by the algorithm"
if (c0 is not None):
assert np.asarray(c0).squeeze().shape[0] == 3, "CoM has not size 3"
self._c0 = np.asarray(c0).squeeze().copy()
if (dc0 is not None):
assert np.asarray(
dc0).squeeze().shape[0] == 3, "CoM velocity has not size 3"
self._dc0 = np.asarray(dc0).squeeze().copy()
if ((c0 is not None) or (dc0 is not None)):
init_bezier(self._c0, self._dc0, self._n)
''' Solve the linear program
minimize c' x
subject to Alb <= A_in x <= Aub
A_eq x = b
lb <= x <= ub
Return a tuple containing:
status flag
primal solution
dual solution
'''
use_angular_momentum = l0 != None
# for the moment c is random stuff.
dim_pb = 6 if use_angular_momentum else 3
c = zeros(dim_pb)
c[2] = -1
wps = self.compute_6d_control_point_inequalities(T, time_step, l0)
status, x, cost, comp_time = self._solve(dim_pb, l0, asLp)
is_stable = status == LP_status.LP_STATUS_OPTIMAL
wps = [self._p0, self._p1, x[:3], x[:3]]
wpsL = [
zeros(3) if not use_angular_momentum else l0[:],
zeros(3) if not use_angular_momentum else x[-3:],
zeros(3),
zeros(3)
]
wpsdL = [3 * (wpsL[1] - wpsL[0]), 3 * (-wpsL[1]),
zeros(3)]
c_of_s = bezier(matrix([pi.tolist() for pi in wps]).transpose())
dc_of_s = c_of_s.compute_derivate(1)
ddc_of_s = c_of_s.compute_derivate(2)
dL_of_s = bezier(matrix([pi.tolist() for pi in wpsdL]).transpose())
L_of_s = bezier(matrix([pi.tolist() for pi in wpsL]).transpose())
return Bunch(is_stable=is_stable,
c=x[:3],
dc=zeros(3),
computation_time=comp_time,
ddc_min=0.0,
t=T,
c_of_t=c_of_t(c_of_s, T),
dc_of_t=dc_of_t(dc_of_s, T),
ddc_of_t=c_of_t(ddc_of_s, T),
dL_of_t=dc_of_t(dL_of_s, T),
L_of_t=c_of_t(L_of_s, T),
cost=cost,
wps=wps,
wpsL=wpsL,
wpsdL=wpsdL)
def test_all(self):
waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
waypoints6 = matrix([[1., 2., 3., 7., 5., 5.],
[4., 5., 6., 4., 5., 6.]]).transpose()
time_waypoints = matrix([0., 1.])
# testing bezier curve
a = bezier6(waypoints6)
a = bezier(waypoints, -1., 3.)
self.assertEqual(a.degree, a.nbWaypoints - 1)
a.min()
a.max()
a(0.4)
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)
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())
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())
# testing bezier with constraints
c = curve_constraints()
c.init_vel = matrix([0., 1., 1.])
c.end_vel = matrix([0., 1., 1.])
c.init_acc = matrix([0., 1., -1.])
c.end_acc = matrix([0., 100., 1.])
a = bezier(waypoints, c)
self.assertLess(norm(a.derivate(0, 1) - c.init_vel), 1e-10)
self.assertLess(norm(a.derivate(1, 2) - c.end_acc), 1e-10)
# testing polynom function
a = polynom(waypoints)
a = polynom(waypoints, -1., 3.)
a.min()
a.max()
a(0.4)
self.assertTrue(((a.derivate(0.4, 0) == a(0.4)).all()))
a.derivate(0.4, 2)
# testing exact_cubic function
a = exact_cubic(waypoints, time_waypoints)
a.min()
a.max()
a(0.4)
self.assertTrue(((a.derivate(0.4, 0) == a(0.4)).all()))
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.])
c.end_vel = matrix([0., 1., 1.])
c.init_acc = matrix([0., 1., 1.])
c.end_acc = matrix([0., 1., 1.])
a = spline_deriv_constraint(waypoints, time_waypoints)
a = spline_deriv_constraint(waypoints, time_waypoints, c)
from spline import bezier, bezier6, polynom, exact_cubic, curve_constraints, spline_deriv_constraint, from_bezier from numpy import matrix from numpy.linalg import norm __EPS = 1e-6 waypoints = matrix([[1.,2.,3.],[4.,5.,6.]]).transpose() waypoints6 = matrix([[1.,2.,3.,7.,5.,5.],[4.,5.,6.,4.,5.,6.]]).transpose() time_waypoints = matrix([0.,1.]) #testing bezier curve a = bezier6(waypoints6) a = bezier(waypoints, 3.) assert(a.degree == a.nbWaypoints -1) a.min() a.max() a(0.4) assert((a.derivate(0.4,0) == a(0.4)).all()) 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(a(t) == prim.derivate(t,1)).all() assert(prim(0) == matrix([0.,0.,0.])).all()
