def make_contact_assignment(dt, num_time_steps, constrained_limbs=None):
"""
@inputs
constrained_limbs: should be a dict with limb_name and value to which it is constrained
default:
both feet to always be in contact,
right hand never to be in contact.
"""
if constrained_limbs is None:
constrained_limbs = dict()
constrained_limbs["right_leg"] = 1 # persistent contact
constrained_limbs["left_leg"] = 1 # persistent contact
constrained_limbs["right_arm"] = 0 # not in contact
time_horizon = num_time_steps * dt
ts = np.linspace(0, time_horizon, time_horizon / dt + 1)
domain = ts
limb_idx_map = ContactStabilizationUtils.limb_idx_map
contact_assignments = [None] * len(limb_idx_map)
for limb_name, val in constrained_limbs.iteritems():
limb_idx = limb_idx_map[limb_name]
contact_assignments[limb_idx] = Piecewise(domain, [
Polynomial(np.array([[val]])) for j in range(len(domain) - 1)
])
return contact_assignments
def new_piecewise_polynomial_variable(self,
domain,
dimension,
degree,
kind="continuous"):
if kind == "continuous":
C = [
self.NewContinuousVariables(degree + 1,
len(domain) - 1)
for _ in range(dimension)
]
elif kind == "binary":
C = [
self.NewBinaryVariables(degree + 1,
len(domain) - 1)
for _ in range(dimension)
]
else:
raise ValueError(
"Expected kind to be 'continuous' or 'binary', but got {:s}".
format(kind))
C = np.stack(C, axis=2)
assert C.shape == (degree + 1, len(domain) - 1, dimension)
return Piecewise(domain, [
Polynomial([C[i, j, :] for i in range(degree + 1)])
for j in range(len(domain) - 1)
])
def constructContactPiecewisePolynomial(ts, contact_lambda,
contact_sequence_array):
contact_piecewise_functions = []
for i in xrange(len(ts) - 1):
# value at knot point 'i' is sum of values from different lambas
val = np.dot(contact_sequence_array[:, i], contact_lambda)
# have to make the value into a polynomial so that it is callable
polynomial_val = Polynomial(np.array([[val]]))
contact_piecewise_functions.append(polynomial_val)
contact = Piecewise(ts, contact_piecewise_functions)
return contact
def main(entry):
polynomial = Polynomial(entry)
polynomial.parse_entry()
polynomial.get_reduced_form()
polynomial.get_degree()
polynomial.solve()
polynomial.print_reduced_form()
polynomial.print_degree()
polynomial.print_solution()
def test_polynomial_eval(self):
p = Polynomial([1, 2, 3, 4])
for x in range(-5, 5):
self.assertAlmostEqual(p(x), 1 + 2 * x + 3 * x**2 + 4 * x**3)
def test_polynomial_map(self):
p = Polynomial([1, 2, 3, 4])
p2 = p.map(lambda x: x + 1)
self.assertEqual(p2.coeffs, [2, 3, 4, 5])
def test_polynomial_derivative(self):
p = Polynomial([3, 1, 7, 8, 2])
dp = p.derivative()
self.assertEqual(dp.coeffs, [1, 2 * 7, 3 * 8, 4 * 2])
def test_polynomial_degree(self):
p = Polynomial([1, 2, 3, 4])
self.assertEqual(p.degree, 3)