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 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)