def __init__(self, sample_times, num_vars, degree, continuity_degree):
self.sample_times = sample_times
self.n = num_vars
self.degree = degree
self.prog = MathematicalProgram()
self.coeffs = []
for i in range(len(sample_times)):
self.coeffs.append(
self.prog.NewContinuousVariables(num_vars, degree + 1, "C"))
self.result = None
# Add continuity constraints
for s in range(len(sample_times) - 1):
trel = sample_times[s + 1] - sample_times[s]
coeffs = self.coeffs[s]
for var in range(self.n):
for deg in range(continuity_degree + 1):
# Don't use eval here, because I want left and right
# values of the same time
left_val = 0
for d in range(deg, self.degree + 1):
left_val += coeffs[var, d]*np.power(trel, d-deg) * \
math.factorial(d)/math.factorial(d-deg)
right_val = self.coeffs[s + 1][var,
deg] * math.factorial(deg)
self.prog.AddLinearConstraint(left_val == right_val)
# Add cost to minimize highest order terms
for s in range(len(sample_times) - 1):
self.prog.AddQuadraticCost(np.eye(num_vars), np.zeros(
(num_vars, 1)), self.coeffs[s][:, -1])
def __init__(self, *args, **kwargs):
super().__init__(*args, **kwargs)
self.A = np.eye(3)
self.b = [1.0, 1.0, 1.0]
self.prog = MathematicalProgram()
self.x = self.prog.NewContinuousVariables(3, "x")
self.t = self.prog.NewContinuousVariables(1, "t")
def build_empty_mp(self):
# Construct the problem
mp = MathematicalProgram()
xyz = mp.NewContinuousVariables(3, 'xyz')
rpy = mp.NewContinuousVariables(3, 'rpy')
xyz_rpy = np.concatenate((xyz, rpy))
mp.SetInitialGuessForAllVariables(np.zeros(6))
# Store the result to problem
self.mp = mp
self.xyzrpy = xyz_rpy
def __init__(self, *args, **kwargs):
super().__init__(*args, **kwargs)
self.A = np.eye(3)
self.b = [1.0, 1.0, 1.0]
self.prog = MathematicalProgram()
self.x = self.prog.NewContinuousVariables(3, "x")
self.t = self.prog.NewContinuousVariables(1, "t")[0]
self.Ay = np.array([[1., 0.], [0., 1.], [1., 0.]])
self.by = np.ones(3)
self.cz = np.ones(2)
self.dz = 1.
self.y = self.prog.NewContinuousVariables(2, "y")
self.z = self.prog.NewContinuousVariables(2, "z")
def find_feasible_latents(self, observed):
# Build an optimization to estimate the hidden variables
try:
prog = MathematicalProgram()
# Add in all the appropriate variables with their bounds
all_vars = self.prices[0].GetVariables()
for price in self.prices[1:]:
all_vars += price.GetVariables()
mp_vars = prog.NewContinuousVariables(len(all_vars))
subs_dict = {}
for v, mv in zip(all_vars, mp_vars):
subs_dict[v] = mv
lb = []
ub = []
prog.AddBoundingBoxConstraint(0., 1., mp_vars)
prices_mp = [
self.prices[k].Substitute(subs_dict) for k in range(12)
]
# Add the observation constraint
for k, val in enumerate(observed[1:]):
if val != 0:
prog.AddConstraint(prices_mp[k] >= val - 2.)
prog.AddConstraint(prices_mp[k] <= val + 2)
# Find lower bounds
prog.AddCost(sum(prices_mp))
solver = SnoptSolver()
result = solver.Solve(prog)
if result.is_success():
lb = [result.GetSolution(x).Evaluate() for x in prices_mp]
lb_vars = result.GetSolution(mp_vars)
# Find upper bound too
prog.AddCost(-2. * sum(prices_mp))
result = solver.Solve(prog)
if result.is_success():
ub_vars = result.GetSolution(mp_vars)
ub = [result.GetSolution(x).Evaluate() for x in prices_mp]
self.price_ub = ub
self.price_lb = lb
subs_dict = {}
for k, v in enumerate(all_vars):
if lb_vars[k] == ub_vars[k]:
subs_dict[v] = lb_vars[k]
else:
new_var = sym.Variable(
"feasible_%d" % k,
sym.Variable.Type.RANDOM_UNIFORM)
subs_dict[v] = new_var * (ub_vars[k] -
lb_vars[k]) + lb_vars[k]
self.prices = [
self.prices[k].Substitute(subs_dict) for k in range(12)
]
return
except RuntimeError as e:
print("Runtime error: ", e)
self.rollout_probability = 0.
def main():
parser = ArgumentParser()
parser.add_argument('--datapath', default='../out/data.npy')
parser.add_argument('--normalize', dest='normalize', action='store_true')
parser.add_argument('--no-normalize',
dest='normalize',
action='store_false')
parser.set_defaults(normalize=True)
opts = parser.parse_args()
data = dynamics.unmarshal_data(np.load(opts.datapath))
n = data.poslambdas.shape[0]
lambdas = np.vstack((data.poslambdas, data.neglambdas, data.gammas))
ds = np.vstack((data.xdots, data.us, np.ones(data.xdots.shape)))
mp = MathematicalProgram()
M = mp.NewContinuousVariables(3, 3, "M")
W = mp.NewContinuousVariables(3, 3, "W")
W[0, 2] = 0
W[1, 2] = 0
R = M.dot(lambdas) + W.dot(ds)
elementwise_positive_constraint(mp, R)
lambdas_vec = np.expand_dims(np.ndarray.flatten(lambdas), 0)
R_vec = np.ndarray.flatten(R)
mp.AddLinearCost(lambdas_vec.dot(R_vec)[0])
result = Solve(mp)
print(result.is_success())
Msol = result.GetSolution(M)
# When mix 0's in with W, need to evaluate expression
evaluator = np.vectorize(lambda x: x.Evaluate())
Wsol = evaluator(result.GetSolution(W))
print(Msol)
print(Wsol)
def findMaxForce(theta1, theta2, theta3):
theta12 = theta2 + theta1
theta123 = theta3 + theta12
s1 = sin(theta1)
s2 = sin(theta2)
s3 = sin(theta3)
s12 = sin(theta12)
s123 = sin(theta123)
c1 = cos(theta1)
c2 = cos(theta2)
c3 = cos(theta3)
c12 = cos(theta12)
c123 = cos(theta123)
J = np.array([[
-l_1 * s1 - l_2 * s12 - l_3 * s123, -l_2 * s12 - l_3 * s123,
-l_3 * s123
], [l_1 * c1 + l_2 * c12 + l_3 * c123, l_2 * c12 + l_3 * c123,
l_3 * c123]])
tau1 = (
-l_1 * c1 * m_2 * g # torque due to l_1 - l_2 link motor
+ (-l_1 * c1 + l_2 / 2.0 * c12) * m_m *
g # torque due to battery link motor
+ (-l_1 * c1 + l_2 / 2.0 * c12 + l_b * cos(theta12 + np.pi / 2.0)) *
m_b * g # torque due to battery
+ (-l_1 * c1 + l_2) * m_3 * g # torque due to l_2 - l_3 link motor
+ (-l_1 * c1 + l_2 + l_3 * c3) * m_w * g # torque due to front wheel
)
# print("tau1 = " + str(tau1))
mp = MathematicalProgram()
tau23 = mp.NewContinuousVariables(2, "tau23")
tau123 = np.append(tau1, tau23)
mp.AddCost(J.dot(tau123)[0])
mp.AddConstraint(tau23[0]**2 <= LINK_MOTOR_CONTINUOUS_STALL_TORQUE**2)
mp.AddConstraint(tau23[1]**2 <= LINK_MOTOR_CONTINUOUS_STALL_TORQUE**2)
mp.AddLinearConstraint(J.dot(tau123)[1] >= -HUB_MOTOR_FORCE / 2.0)
result = Solve(mp)
is_success = result.is_success()
torques = result.GetSolution(tau23)
full_tau = np.append(tau1, torques)
output_force = J.dot(full_tau)
# print("is_success = " + str(is_success))
# print("tau = " + str(full_tau))
# print("F = " + str(J.dot(full_tau)))
return is_success, output_force, torques
def minimize_height(diagram_f, plant_f, d_context_f, frame_list):
"""Fragile and slow :("""
context_f = plant_f.GetMyContextFromRoot(d_context_f)
diagram_ad = diagram_f.ToAutoDiffXd()
plant_ad = diagram_ad.GetSubsystemByName(plant_f.get_name())
d_context_ad = diagram_ad.CreateDefaultContext()
d_context_ad.SetTimeStateAndParametersFrom(d_context_f)
context_ad = plant_ad.GetMyContextFromRoot(d_context_ad)
def prepare_plant_and_context(z):
if z.dtype == float:
plant, context = plant_f, context_f
else:
plant, context = plant_ad, context_ad
set_frame_heights(plant, context, frame_list, z)
return plant, context
prog = MathematicalProgram()
num_z = len(frame_list)
z_vars = prog.NewContinuousVariables(num_z, "z")
q0 = plant_f.GetPositions(context_f)
z0 = get_frame_heights(plant_f, context_f, frame_list)
cost = prog.AddCost(np.sum(z_vars))
prog.AddBoundingBoxConstraint([0.01] * num_z, [5.] * num_z, z_vars)
# # N.B. Cannot use AutoDiffXd due to cylinders.
distance = MinimumDistanceConstraint(plant=plant_f,
plant_context=context_f,
minimum_distance=0.05)
def distance_with_z(z):
plant, context = prepare_plant_and_context(z)
q = plant.GetPositions(context)
return distance.Eval(q)
prog.AddConstraint(distance_with_z,
lb=distance.lower_bound(),
ub=distance.upper_bound(),
vars=z_vars)
result = Solve(prog, initial_guess=z0)
assert result.is_success()
z = result.GetSolution(z_vars)
set_frame_heights(plant_f, context_f, frame_list, z)
q = plant_f.GetPositions(context_f)
return q
def test_optimization(self):
"""Tests geometry::optimization bindings"""
A = np.eye(3)
b = [1.0, 1.0, 1.0]
prog = MathematicalProgram()
x = prog.NewContinuousVariables(3, "x")
t = prog.NewContinuousVariables(1, "t")
# Test Point.
p = np.array([11.1, 12.2, 13.3])
point = mut.optimization.Point(p)
self.assertEqual(point.ambient_dimension(), 3)
np.testing.assert_array_equal(point.x(), p)
point.set_x(x=2*p)
np.testing.assert_array_equal(point.x(), 2*p)
point.set_x(x=p)
# Test HPolyhedron.
hpoly = mut.optimization.HPolyhedron(A=A, b=b)
self.assertEqual(hpoly.ambient_dimension(), 3)
np.testing.assert_array_equal(hpoly.A(), A)
np.testing.assert_array_equal(hpoly.b(), b)
self.assertTrue(hpoly.PointInSet(x=[0, 0, 0], tol=0.0))
hpoly.AddPointInSetConstraints(prog, x)
with self.assertRaisesRegex(
RuntimeError, ".*not implemented yet for HPolyhedron.*"):
hpoly.ToShapeWithPose()
h_box = mut.optimization.HPolyhedron.MakeBox(
lb=[-1, -1, -1], ub=[1, 1, 1])
h_unit_box = mut.optimization.HPolyhedron.MakeUnitBox(dim=3)
np.testing.assert_array_equal(h_box.A(), h_unit_box.A())
np.testing.assert_array_equal(h_box.b(), h_unit_box.b())
self.assertIsInstance(
h_box.MaximumVolumeInscribedEllipsoid(),
mut.optimization.Hyperellipsoid)
np.testing.assert_array_almost_equal(
h_box.ChebyshevCenter(), [0, 0, 0])
# Test Hyperellipsoid.
ellipsoid = mut.optimization.Hyperellipsoid(A=A, center=b)
self.assertEqual(ellipsoid.ambient_dimension(), 3)
np.testing.assert_array_equal(ellipsoid.A(), A)
np.testing.assert_array_equal(ellipsoid.center(), b)
self.assertTrue(ellipsoid.PointInSet(x=b, tol=0.0))
ellipsoid.AddPointInSetConstraints(prog, x)
shape, pose = ellipsoid.ToShapeWithPose()
self.assertIsInstance(shape, mut.Ellipsoid)
self.assertIsInstance(pose, RigidTransform)
scale, witness = ellipsoid.MinimumUniformScalingToTouch(point)
self.assertTrue(scale > 0.0)
np.testing.assert_array_almost_equal(witness, p)
e_ball = mut.optimization.Hyperellipsoid.MakeAxisAligned(
radius=[1, 1, 1], center=b)
np.testing.assert_array_equal(e_ball.A(), A)
np.testing.assert_array_equal(e_ball.center(), b)
e_ball2 = mut.optimization.Hyperellipsoid.MakeHypersphere(
radius=1, center=b)
np.testing.assert_array_equal(e_ball2.A(), A)
np.testing.assert_array_equal(e_ball2.center(), b)
e_ball3 = mut.optimization.Hyperellipsoid.MakeUnitBall(dim=3)
np.testing.assert_array_equal(e_ball3.A(), A)
np.testing.assert_array_equal(e_ball3.center(), [0, 0, 0])
# Test VPolytope.
vertices = np.array([[0.0, 1.0, 2.0], [3.0, 7.0, 5.0]])
vpoly = mut.optimization.VPolytope(vertices=vertices)
self.assertEqual(vpoly.ambient_dimension(), 2)
np.testing.assert_array_equal(vpoly.vertices(), vertices)
self.assertTrue(vpoly.PointInSet(x=[1.0, 5.0], tol=1e-8))
vpoly.AddPointInSetConstraints(prog, x[0:2])
v_box = mut.optimization.VPolytope.MakeBox(
lb=[-1, -1, -1], ub=[1, 1, 1])
self.assertTrue(v_box.PointInSet([0, 0, 0]))
v_unit_box = mut.optimization.VPolytope.MakeUnitBox(dim=3)
self.assertTrue(v_unit_box.PointInSet([0, 0, 0]))
# Test remaining ConvexSet methods using these instances.
self.assertIsInstance(hpoly.Clone(), mut.optimization.HPolyhedron)
self.assertTrue(ellipsoid.IsBounded())
hpoly.AddPointInNonnegativeScalingConstraints(prog=prog, x=x, t=t[0])
# Test MakeFromSceneGraph methods.
scene_graph = mut.SceneGraph()
source_id = scene_graph.RegisterSource("source")
frame_id = scene_graph.RegisterFrame(
source_id=source_id, frame=mut.GeometryFrame("frame"))
box_geometry_id = scene_graph.RegisterGeometry(
source_id=source_id, frame_id=frame_id,
geometry=mut.GeometryInstance(X_PG=RigidTransform(),
shape=mut.Box(1., 1., 1.),
name="sphere"))
sphere_geometry_id = scene_graph.RegisterGeometry(
source_id=source_id, frame_id=frame_id,
geometry=mut.GeometryInstance(X_PG=RigidTransform(),
shape=mut.Sphere(1.), name="sphere"))
context = scene_graph.CreateDefaultContext()
pose_vector = mut.FramePoseVector()
pose_vector.set_value(frame_id, RigidTransform())
scene_graph.get_source_pose_port(source_id).FixValue(
context, pose_vector)
query_object = scene_graph.get_query_output_port().Eval(context)
H = mut.optimization.HPolyhedron(
query_object=query_object, geometry_id=box_geometry_id,
reference_frame=scene_graph.world_frame_id())
self.assertEqual(H.ambient_dimension(), 3)
E = mut.optimization.Hyperellipsoid(
query_object=query_object, geometry_id=sphere_geometry_id,
reference_frame=scene_graph.world_frame_id())
self.assertEqual(E.ambient_dimension(), 3)
P = mut.optimization.Point(
query_object=query_object, geometry_id=sphere_geometry_id,
reference_frame=scene_graph.world_frame_id(),
maximum_allowable_radius=1.5)
self.assertEqual(P.ambient_dimension(), 3)
V = mut.optimization.VPolytope(
query_object=query_object, geometry_id=box_geometry_id,
reference_frame=scene_graph.world_frame_id())
self.assertEqual(V.ambient_dimension(), 3)
# Test Iris.
obstacles = mut.optimization.MakeIrisObstacles(
query_object=query_object,
reference_frame=scene_graph.world_frame_id())
options = mut.optimization.IrisOptions()
options.require_sample_point_is_contained = True
options.iteration_limit = 1
options.termination_threshold = 0.1
region = mut.optimization.Iris(
obstacles=obstacles, sample=[2, 3.4, 5],
domain=mut.optimization.HPolyhedron.MakeBox(
lb=[-5, -5, -5], ub=[5, 5, 5]), options=options)
self.assertIsInstance(region, mut.optimization.HPolyhedron)
obstacles = [
mut.optimization.HPolyhedron.MakeUnitBox(3),
mut.optimization.Hyperellipsoid.MakeUnitBall(3),
mut.optimization.Point([0, 0, 0]),
mut.optimization.VPolytope.MakeUnitBox(3)]
region = mut.optimization.Iris(
obstacles=obstacles, sample=[2, 3.4, 5],
domain=mut.optimization.HPolyhedron.MakeBox(
lb=[-5, -5, -5], ub=[5, 5, 5]), options=options)
self.assertIsInstance(region, mut.optimization.HPolyhedron)
class TestGeometryOptimization(unittest.TestCase):
def __init__(self, *args, **kwargs):
super().__init__(*args, **kwargs)
self.A = np.eye(3)
self.b = [1.0, 1.0, 1.0]
self.prog = MathematicalProgram()
self.x = self.prog.NewContinuousVariables(3, "x")
self.t = self.prog.NewContinuousVariables(1, "t")
def test_point_convex_set(self):
p = np.array([11.1, 12.2, 13.3])
point = mut.Point(p)
self.assertEqual(point.ambient_dimension(), 3)
np.testing.assert_array_equal(point.x(), p)
point.set_x(x=2*p)
np.testing.assert_array_equal(point.x(), 2*p)
point.set_x(x=p)
# TODO(SeanCurtis-TRI): This doesn't test the constructor that
# builds from shape.
def test_h_polyhedron(self):
hpoly = mut.HPolyhedron(A=self.A, b=self.b)
self.assertEqual(hpoly.ambient_dimension(), 3)
np.testing.assert_array_equal(hpoly.A(), self.A)
np.testing.assert_array_equal(hpoly.b(), self.b)
self.assertTrue(hpoly.PointInSet(x=[0, 0, 0], tol=0.0))
self.assertFalse(hpoly.IsBounded())
hpoly.AddPointInSetConstraints(self.prog, self.x)
with self.assertRaisesRegex(
RuntimeError, ".*not implemented yet for HPolyhedron.*"):
hpoly.ToShapeWithPose()
h_box = mut.HPolyhedron.MakeBox(
lb=[-1, -1, -1], ub=[1, 1, 1])
self.assertTrue(h_box.IntersectsWith(hpoly))
h_unit_box = mut.HPolyhedron.MakeUnitBox(dim=3)
np.testing.assert_array_equal(h_box.A(), h_unit_box.A())
np.testing.assert_array_equal(h_box.b(), h_unit_box.b())
self.assertIsInstance(
h_box.MaximumVolumeInscribedEllipsoid(),
mut.Hyperellipsoid)
np.testing.assert_array_almost_equal(
h_box.ChebyshevCenter(), [0, 0, 0])
h2 = h_box.CartesianProduct(other=h_unit_box)
self.assertIsInstance(h2, mut.HPolyhedron)
self.assertEqual(h2.ambient_dimension(), 6)
h3 = h_box.CartesianPower(n=3)
self.assertIsInstance(h3, mut.HPolyhedron)
self.assertEqual(h3.ambient_dimension(), 9)
def test_hyper_ellipsoid(self):
ellipsoid = mut.Hyperellipsoid(A=self.A, center=self.b)
self.assertEqual(ellipsoid.ambient_dimension(), 3)
np.testing.assert_array_equal(ellipsoid.A(), self.A)
np.testing.assert_array_equal(ellipsoid.center(), self.b)
self.assertTrue(ellipsoid.PointInSet(x=self.b, tol=0.0))
ellipsoid.AddPointInSetConstraints(self.prog, self.x)
shape, pose = ellipsoid.ToShapeWithPose()
self.assertIsInstance(shape, Ellipsoid)
self.assertIsInstance(pose, RigidTransform)
p = np.array([11.1, 12.2, 13.3])
point = mut.Point(p)
scale, witness = ellipsoid.MinimumUniformScalingToTouch(point)
self.assertTrue(scale > 0.0)
np.testing.assert_array_almost_equal(witness, p)
e_ball = mut.Hyperellipsoid.MakeAxisAligned(
radius=[1, 1, 1], center=self.b)
np.testing.assert_array_equal(e_ball.A(), self.A)
np.testing.assert_array_equal(e_ball.center(), self.b)
e_ball2 = mut.Hyperellipsoid.MakeHypersphere(
radius=1, center=self.b)
np.testing.assert_array_equal(e_ball2.A(), self.A)
np.testing.assert_array_equal(e_ball2.center(), self.b)
e_ball3 = mut.Hyperellipsoid.MakeUnitBall(dim=3)
np.testing.assert_array_equal(e_ball3.A(), self.A)
np.testing.assert_array_equal(e_ball3.center(), [0, 0, 0])
def test_minkowski_sum(self):
point = mut.Point(np.array([11.1, 12.2, 13.3]))
hpoly = mut.HPolyhedron(A=self.A, b=self.b)
sum = mut.MinkowskiSum(setA=point, setB=hpoly)
self.assertEqual(sum.ambient_dimension(), 3)
self.assertEqual(sum.num_terms(), 2)
sum2 = mut.MinkowskiSum(sets=[point, hpoly])
self.assertEqual(sum2.ambient_dimension(), 3)
self.assertEqual(sum2.num_terms(), 2)
self.assertIsInstance(sum2.term(0), mut.Point)
def test_v_polytope(self):
vertices = np.array([[0.0, 1.0, 2.0], [3.0, 7.0, 5.0]])
vpoly = mut.VPolytope(vertices=vertices)
self.assertEqual(vpoly.ambient_dimension(), 2)
np.testing.assert_array_equal(vpoly.vertices(), vertices)
self.assertTrue(vpoly.PointInSet(x=[1.0, 5.0], tol=1e-8))
vpoly.AddPointInSetConstraints(self.prog, self.x[0:2])
v_box = mut.VPolytope.MakeBox(
lb=[-1, -1, -1], ub=[1, 1, 1])
self.assertTrue(v_box.PointInSet([0, 0, 0]))
self.assertAlmostEqual(v_box.CalcVolume(), 8, 1E-10)
v_unit_box = mut.VPolytope.MakeUnitBox(dim=3)
self.assertTrue(v_unit_box.PointInSet([0, 0, 0]))
v_from_h = mut.VPolytope(H=mut.HPolyhedron.MakeUnitBox(dim=3))
self.assertTrue(v_from_h.PointInSet([0, 0, 0]))
def test_cartesian_product(self):
point = mut.Point(np.array([11.1, 12.2, 13.3]))
h_box = mut.HPolyhedron.MakeBox(
lb=[-1, -1, -1], ub=[1, 1, 1])
sum = mut.CartesianProduct(setA=point, setB=h_box)
self.assertEqual(sum.ambient_dimension(), 6)
self.assertEqual(sum.num_factors(), 2)
sum2 = mut.CartesianProduct(sets=[point, h_box])
self.assertEqual(sum2.ambient_dimension(), 6)
self.assertEqual(sum2.num_factors(), 2)
self.assertIsInstance(sum2.factor(0), mut.Point)
sum2 = mut.CartesianProduct(
sets=[point, h_box], A=np.eye(6, 3), b=[0, 1, 2, 3, 4, 5])
self.assertEqual(sum2.ambient_dimension(), 3)
self.assertEqual(sum2.num_factors(), 2)
self.assertIsInstance(sum2.factor(1), mut.HPolyhedron)
def test_make_from_scene_graph_and_iris(self):
"""
Tests the make from scene graph and iris functionality together as
the Iris code makes obstacles from geometries registered in SceneGraph.
"""
scene_graph = SceneGraph()
source_id = scene_graph.RegisterSource("source")
frame_id = scene_graph.RegisterFrame(
source_id=source_id, frame=GeometryFrame("frame"))
box_geometry_id = scene_graph.RegisterGeometry(
source_id=source_id, frame_id=frame_id,
geometry=GeometryInstance(X_PG=RigidTransform(),
shape=Box(1., 1., 1.),
name="box"))
cylinder_geometry_id = scene_graph.RegisterGeometry(
source_id=source_id, frame_id=frame_id,
geometry=GeometryInstance(X_PG=RigidTransform(),
shape=Cylinder(1., 1.),
name="cylinder"))
sphere_geometry_id = scene_graph.RegisterGeometry(
source_id=source_id, frame_id=frame_id,
geometry=GeometryInstance(X_PG=RigidTransform(),
shape=Sphere(1.), name="sphere"))
capsule_geometry_id = scene_graph.RegisterGeometry(
source_id=source_id,
frame_id=frame_id,
geometry=GeometryInstance(X_PG=RigidTransform(),
shape=Capsule(1., 1.0),
name="capsule"))
context = scene_graph.CreateDefaultContext()
pose_vector = FramePoseVector()
pose_vector.set_value(frame_id, RigidTransform())
scene_graph.get_source_pose_port(source_id).FixValue(
context, pose_vector)
query_object = scene_graph.get_query_output_port().Eval(context)
H = mut.HPolyhedron(
query_object=query_object, geometry_id=box_geometry_id,
reference_frame=scene_graph.world_frame_id())
self.assertEqual(H.ambient_dimension(), 3)
C = mut.CartesianProduct(
query_object=query_object, geometry_id=cylinder_geometry_id,
reference_frame=scene_graph.world_frame_id())
self.assertEqual(C.ambient_dimension(), 3)
E = mut.Hyperellipsoid(
query_object=query_object, geometry_id=sphere_geometry_id,
reference_frame=scene_graph.world_frame_id())
self.assertEqual(E.ambient_dimension(), 3)
S = mut.MinkowskiSum(
query_object=query_object, geometry_id=capsule_geometry_id,
reference_frame=scene_graph.world_frame_id())
self.assertEqual(S.ambient_dimension(), 3)
P = mut.Point(
query_object=query_object, geometry_id=sphere_geometry_id,
reference_frame=scene_graph.world_frame_id(),
maximum_allowable_radius=1.5)
self.assertEqual(P.ambient_dimension(), 3)
V = mut.VPolytope(
query_object=query_object, geometry_id=box_geometry_id,
reference_frame=scene_graph.world_frame_id())
self.assertEqual(V.ambient_dimension(), 3)
obstacles = mut.MakeIrisObstacles(
query_object=query_object,
reference_frame=scene_graph.world_frame_id())
options = mut.IrisOptions()
options.require_sample_point_is_contained = True
options.iteration_limit = 1
options.termination_threshold = 0.1
options.relative_termination_threshold = 0.01
self.assertNotIn("object at 0x", repr(options))
region = mut.Iris(
obstacles=obstacles, sample=[2, 3.4, 5],
domain=mut.HPolyhedron.MakeBox(
lb=[-5, -5, -5], ub=[5, 5, 5]), options=options)
self.assertIsInstance(region, mut.HPolyhedron)
obstacles = [
mut.HPolyhedron.MakeUnitBox(3),
mut.Hyperellipsoid.MakeUnitBall(3),
mut.Point([0, 0, 0]),
mut.VPolytope.MakeUnitBox(3)]
region = mut.Iris(
obstacles=obstacles, sample=[2, 3.4, 5],
domain=mut.HPolyhedron.MakeBox(
lb=[-5, -5, -5], ub=[5, 5, 5]), options=options)
self.assertIsInstance(region, mut.HPolyhedron)
def test_iris_cspace(self):
limits_urdf = """
<robot name="limits">
<link name="movable">
<collision>
<geometry><box size="1 1 1"/></geometry>
</collision>
</link>
<joint name="movable" type="prismatic">
<axis xyz="1 0 0"/>
<limit lower="-2" upper="2"/>
<parent link="world"/>
<child link="movable"/>
</joint>
</robot>"""
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
Parser(plant).AddModelFromString(limits_urdf, "urdf")
plant.Finalize()
diagram = builder.Build()
context = diagram.CreateDefaultContext()
options = mut.IrisOptions()
with catch_drake_warnings(expected_count=1):
region = mut.IrisInConfigurationSpace(
plant=plant, context=plant.GetMyContextFromRoot(context),
sample=[0], options=options)
plant.SetPositions(plant.GetMyMutableContextFromRoot(context), [0])
region = mut.IrisInConfigurationSpace(
plant=plant, context=plant.GetMyContextFromRoot(context),
options=options)
self.assertIsInstance(region, mut.ConvexSet)
self.assertEqual(region.ambient_dimension(), 1)
self.assertTrue(region.PointInSet([1.0]))
self.assertFalse(region.PointInSet([3.0]))
def test_graph_of_convex_sets(self):
spp = mut.GraphOfConvexSets()
source = spp.AddVertex(set=mut.Point([0.1]), name="source")
target = spp.AddVertex(set=mut.Point([0.2]), name="target")
edge0 = spp.AddEdge(u=source, v=target, name="edge0")
edge1 = spp.AddEdge(u_id=source.id(), v_id=target.id(), name="edge1")
self.assertEqual(len(spp.Vertices()), 2)
self.assertEqual(len(spp.Edges()), 2)
result = spp.SolveShortestPath(
source_id=source.id(), target_id=target.id(),
convex_relaxation=True)
self.assertIsInstance(result, MathematicalProgramResult)
self.assertIsInstance(spp.SolveShortestPath(
source=source, target=target, convex_relaxation=True),
MathematicalProgramResult)
self.assertIn("source", spp.GetGraphvizString(
result=result, show_slacks=True, precision=2, scientific=False))
# Vertex
self.assertIsInstance(source.id(), mut.GraphOfConvexSets.VertexId)
self.assertEqual(source.ambient_dimension(), 1)
self.assertEqual(source.name(), "source")
self.assertIsInstance(source.x()[0], Variable)
self.assertIsInstance(source.set(), mut.Point)
np.testing.assert_array_almost_equal(
source.GetSolution(result), [0.1], 1e-6)
# Edge
self.assertAlmostEqual(edge0.GetSolutionCost(result=result), 0.0, 1e-6)
np.testing.assert_array_almost_equal(
edge0.GetSolutionPhiXu(result=result), [0.1], 1e-6)
np.testing.assert_array_almost_equal(
edge0.GetSolutionPhiXv(result=result), [0.2], 1e-6)
self.assertIsInstance(edge0.id(), mut.GraphOfConvexSets.EdgeId)
self.assertEqual(edge0.name(), "edge0")
self.assertEqual(edge0.u(), source)
self.assertEqual(edge0.v(), target)
self.assertIsInstance(edge0.phi(), Variable)
self.assertIsInstance(edge0.xu()[0], Variable)
self.assertIsInstance(edge0.xv()[0], Variable)
var, binding = edge0.AddCost(e=1.0+edge0.xu()[0])
self.assertIsInstance(var, Variable)
self.assertIsInstance(binding, Binding[Cost])
var, binding = edge0.AddCost(binding=binding)
self.assertIsInstance(var, Variable)
self.assertIsInstance(binding, Binding[Cost])
binding = edge0.AddConstraint(f=(edge0.xu()[0] == edge0.xv()[0]))
self.assertIsInstance(binding, Binding[Constraint])
binding = edge0.AddConstraint(binding=binding)
self.assertIsInstance(binding, Binding[Constraint])
edge0.AddPhiConstraint(phi_value=False)
edge0.ClearPhiConstraints()
# Remove Edges
self.assertEqual(len(spp.Edges()), 2)
spp.RemoveEdge(edge1.id())
self.assertEqual(len(spp.Edges()), 1)
spp.RemoveEdge(edge0)
self.assertEqual(len(spp.Edges()), 0)
# Remove Vertices
self.assertEqual(len(spp.Vertices()), 2)
spp.RemoveVertex(source.id())
self.assertEqual(len(spp.Vertices()), 1)
spp.RemoveVertex(target)
self.assertEqual(len(spp.Vertices()), 0)
mp.AddConstraint(
theta1 <= 0.0).evaluator().set_description("Constrain theta1 <= 0.0")
mp.AddConstraint(theta1 >= -np.pi).evaluator().set_description(
"Constrain theta1 >= -np.pi")
mp.AddConstraint(
theta2 >= 0.0).evaluator().set_description("Constrain theta2 >= 0.0")
mp.AddConstraint(theta2 <= np.pi).evaluator().set_description(
"Constrain theta2 <= np.pi")
mp.AddConstraint(
theta3 >= 0.0).evaluator().set_description("Constrain theta3 >= 0.0")
mp.AddConstraint(theta3 <= np.pi).evaluator().set_description(
"Constrain theta3 <= np.pi")
if __name__ == "__main__":
mp = MathematicalProgram()
state_over_time = np.zeros(shape=(NUM_TIME_STEPS, STATE_SIZE),
dtype=pydrake.symbolic.Variable)
tau234_over_time = np.zeros(shape=(NUM_TIME_STEPS, TORQUE_SIZE),
dtype=pydrake.symbolic.Variable)
initial_state = mp.NewContinuousVariables(8, "state_0")
initial_theta1 = initial_state[0]
initial_theta2 = initial_state[1]
initial_theta3 = initial_state[2]
initial_theta4 = initial_state[3]
# Constrain initial velocity to be 0
# for j in range(4, 8):
# mp.AddConstraint(initial_state[j] <= 0.0).evaluator().set_description("Constrain initial_state[%d] <= 0.0" % j)
# mp.AddConstraint(initial_state[j] >= 0.0).evaluator().set_description("Constrain initial_state[%d] >= 0.0" % j)
from pydrake.solvers.mathematicalprogram import MathematicalProgram import numpy as np import matplotlib.pyplot as plt from pydrake.solvers.mathematicalprogram import Solve prog = MathematicalProgram() x = prog.NewContinuousVariables(2) print(x) print(1 + 2 * x[0] + 3 * x[1] + 4 * x[1]) y = prog.NewContinuousVariables(2, "dog") print(y) print(y[0] + y[0] + y[1] * y[1] * y[1]) var_matrix = prog.NewContinuousVariables(3, 2, "A") print(var_matrix) # Add the constraint x(0) * x(1) = 1 to prog prog.AddConstraint(x[0] * x[1] == 1) prog.AddConstraint(x[0] >= 0) prog.AddConstraint(x[0] - x[1] <= 0) prog.AddCost(x[0]**2 + 3) prog.AddCost(x[0] + x[1]) # New optimization program, all the way through to solving prog = MathematicalProgram() x = prog.NewContinuousVariables(2) prog.AddConstraint(x[0] + x[1] == 1)
def plot_sublevelset_expression(ax, e, vertices=51, **kwargs):
"""
Plots the 2D sub-level set e(x) <= 1, which must contain the origin.
Args:
ax: the matplotlib axis to receive the plot
e: a symbolic expression in two variables
vertices: number of sample points along the boundary
kwargs: are passed to the matplotlib fill method
Returns:
the return values from matplotlib's fill command.
"""
x = list(e.GetVariables())
assert len(x) == 2, "e must be an expression in two variables"
# Handle the special case where e is a degree 2 polynomial.
if e.is_polynomial():
p = Polynomial(e)
if p.TotalDegree() == 2:
env = {a: 0 for a in x}
c = e.Evaluate(env)
e1 = e.Jacobian(x)
b = Evaluate(e1, env)
e2 = Jacobian(e1, x)
A = 0.5 * Evaluate(e2, env)
return plot_sublevelset_quadratic(ax, A, b, c, vertices, **kwargs)
# Find the level-set in polar coordinates, by sampling theta and
# root-finding (on the scalar expression) to find a rplus and rminus.
Xplus = np.empty((2, vertices))
Xminus = np.empty((2, vertices))
i = 0
for theta in np.linspace(0, np.pi, vertices):
prog = MathematicalProgram()
r = prog.NewContinuousVariables(1, "r")[0]
env = {x[0]: r * np.cos(theta), x[1]: r * np.sin(theta)}
scalar = e.Substitute(env)
b = prog.AddBoundingBoxConstraint(0, np.inf, r)
prog.AddConstraint(scalar == 1)
prog.AddQuadraticCost([1], [0], [r])
prog.SetInitialGuess(r, 0.1) # or anything non-zero.
result = Solve(prog)
assert result.is_success(), "Failed to find the level set"
rplus = result.GetSolution(r)
Xplus[0, i] = rplus * np.cos(theta)
Xplus[1, i] = rplus * np.sin(theta)
b.evaluator().UpdateLowerBound([-np.inf])
b.evaluator().UpdateUpperBound([0])
prog.SetInitialGuess(r, -0.1) # or anything non-zero.
result = Solve(prog)
assert result.is_success(), "Failed to find the level set"
rminus = result.GetSolution(r)
Xminus[0, i] = rminus * np.cos(theta)
Xminus[1, i] = rminus * np.sin(theta)
i = i + 1
return ax.fill(np.hstack((Xplus[0, :], Xminus[0, :])),
np.hstack((Xplus[1, :], Xminus[1, :])), **kwargs)
def compute_input(self, x, xd, initial_guess=None):
prog = MathematicalProgram()
# Joint configuration states & Contact forces
q = prog.NewContinuousVariables(rows=self.T + 1,
cols=self.nq,
name='q')
v = prog.NewContinuousVariables(rows=self.T + 1,
cols=self.nq,
name='v')
u = prog.NewContinuousVariables(rows=self.T, cols=self.nu, name='u')
contact = prog.NewContinuousVariables(rows=self.T,
cols=self.nf,
name='lambda1')
z = prog.NewBinaryVariables(rows=self.T, cols=self.nf, name='z')
# Add Initial Condition Constraint
prog.AddConstraint(eq(q[0], np.array(x[0:3])))
prog.AddConstraint(eq(v[0], np.array(x[3:6])))
# Add Final Condition Constraint
prog.AddConstraint(eq(q[self.T], np.array(xd[0:3])))
prog.AddConstraint(eq(v[self.T], np.array(xd[3:6])))
prog.AddConstraint(z[0, 0] == 0)
prog.AddConstraint(z[0, 1] == 0)
# Add Dynamics Constraints
for t in range(self.T):
# Add Dynamics Constraints
prog.AddConstraint(
eq(q[t + 1], (q[t] + self.sim.params['h'] * v[t + 1])))
prog.AddConstraint(v[t + 1, 0] == (
v[t, 0] + self.sim.params['h'] *
(-self.sim.params['c'] * v[t, 0] - contact[t, 0] + u[t, 0])))
prog.AddConstraint(v[t + 1,
1] == (v[t, 1] + self.sim.params['h'] *
(-self.sim.params['c'] * v[t, 1] +
contact[t, 0] - contact[t, 1])))
prog.AddConstraint(v[t + 1, 2] == (
v[t, 2] + self.sim.params['h'] *
(-self.sim.params['c'] * v[t, 2] + contact[t, 1] + u[t, 1])))
# Add Contact Constraints with big M = self.contact
prog.AddConstraint(ge(contact[t], 0))
prog.AddConstraint(contact[t, 0] + self.sim.params['k'] *
(q[t, 1] - q[t, 0] - self.sim.params['d']) >= 0)
prog.AddConstraint(contact[t, 1] + self.sim.params['k'] *
(q[t, 2] - q[t, 1] - self.sim.params['d']) >= 0)
# Mixed Integer Constraints
M = self.contact_max
prog.AddConstraint(contact[t, 0] <= M)
prog.AddConstraint(contact[t, 1] <= M)
prog.AddConstraint(contact[t, 0] <= M * z[t, 0])
prog.AddConstraint(contact[t, 1] <= M * z[t, 1])
prog.AddConstraint(
contact[t, 0] + self.sim.params['k'] *
(q[t, 1] - q[t, 0] - self.sim.params['d']) <= M *
(1 - z[t, 0]))
prog.AddConstraint(
contact[t, 1] + self.sim.params['k'] *
(q[t, 2] - q[t, 1] - self.sim.params['d']) <= M *
(1 - z[t, 1]))
prog.AddConstraint(z[t, 0] + z[t, 1] == 1)
# Add Input Constraints. Contact Constraints already enforced in big-M
# prog.AddConstraint(le(u[t], self.input_max))
# prog.AddConstraint(ge(u[t], -self.input_max))
# Add Costs
prog.AddCost(u[t].dot(u[t]))
# Set Initial Guess as empty. Otherwise, start from last solver iteration.
if (type(initial_guess) == type(None)):
initial_guess = np.empty(prog.num_vars())
# Populate initial guess by linearly interpolating between initial
# and final states
#qinit = np.linspace(x[0:3], xd[0:3], self.T + 1)
qinit = np.tile(np.array(x[0:3]), (self.T + 1, 1))
vinit = np.tile(np.array(x[3:6]), (self.T + 1, 1))
uinit = np.tile(np.array([0, 0]), (self.T, 1))
finit = np.tile(np.array([0, 0]), (self.T, 1))
prog.SetDecisionVariableValueInVector(q, qinit, initial_guess)
prog.SetDecisionVariableValueInVector(v, vinit, initial_guess)
prog.SetDecisionVariableValueInVector(u, uinit, initial_guess)
prog.SetDecisionVariableValueInVector(contact, finit,
initial_guess)
# Solve the program
if (self.solver == "ipopt"):
solver_id = IpoptSolver().solver_id()
elif (self.solver == "snopt"):
solver_id = SnoptSolver().solver_id()
elif (self.solver == "osqp"):
solver_id = OsqpSolver().solver_id()
elif (self.solver == "mosek"):
solver_id = MosekSolver().solver_id()
elif (self.solver == "gurobi"):
solver_id = GurobiSolver().solver_id()
solver = MixedIntegerBranchAndBound(prog, solver_id)
#result = solver.Solve(prog, initial_guess)
result = solver.Solve()
if result != result.kSolutionFound:
raise ValueError('Infeasible optimization problem')
sol = result.GetSolution()
q_opt = result.GetSolution(q)
v_opt = result.GetSolution(v)
u_opt = result.GetSolution(u)
f_opt = result.GetSolution(contact)
return sol, q_opt, v_opt, u_opt, f_opt
class PPTrajectory():
def __init__(self, sample_times, num_vars, degree, continuity_degree):
self.sample_times = sample_times
self.n = num_vars
self.degree = degree
self.prog = MathematicalProgram()
self.coeffs = []
for i in range(len(sample_times)):
self.coeffs.append(
self.prog.NewContinuousVariables(num_vars, degree + 1, "C"))
self.result = None
# Add continuity constraints
for s in range(len(sample_times) - 1):
trel = sample_times[s + 1] - sample_times[s]
coeffs = self.coeffs[s]
for var in range(self.n):
for deg in range(continuity_degree + 1):
# Don't use eval here, because I want left and right
# values of the same time
left_val = 0
for d in range(deg, self.degree + 1):
left_val += coeffs[var, d]*np.power(trel, d-deg) * \
math.factorial(d)/math.factorial(d-deg)
right_val = self.coeffs[s + 1][var,
deg] * math.factorial(deg)
self.prog.AddLinearConstraint(left_val == right_val)
# Add cost to minimize highest order terms
for s in range(len(sample_times) - 1):
self.prog.AddQuadraticCost(np.eye(num_vars), np.zeros(
(num_vars, 1)), self.coeffs[s][:, -1])
def eval(self, t, derivative_order=0):
if derivative_order > self.degree:
return 0
s = 0
while s < len(self.sample_times) - 1 and t >= self.sample_times[s + 1]:
s += 1
trel = t - self.sample_times[s]
if self.result is None:
coeffs = self.coeffs[s]
else:
coeffs = self.result.GetSolution(self.coeffs[s])
deg = derivative_order
val = 0 * coeffs[:, 0]
for var in range(self.n):
for d in range(deg, self.degree + 1):
val[var] += coeffs[var, d]*np.power(trel, d-deg) * \
math.factorial(d)/math.factorial(d-deg)
return val
def add_constraint(self, t, derivative_order, lb, ub=None):
'''
Adds a constraint of the form d^deg lb <= x(t) / dt^deg <= ub
'''
if ub is None:
ub = lb
assert (derivative_order <= self.degree)
val = self.eval(t, derivative_order)
self.prog.AddLinearConstraint(val, lb, ub)
def generate(self):
self.result = Solve(self.prog)
assert (self.result.is_success())
class TestGeometryOptimization(unittest.TestCase):
def __init__(self, *args, **kwargs):
super().__init__(*args, **kwargs)
self.A = np.eye(3)
self.b = [1.0, 1.0, 1.0]
self.prog = MathematicalProgram()
self.x = self.prog.NewContinuousVariables(3, "x")
self.t = self.prog.NewContinuousVariables(1, "t")[0]
self.Ay = np.array([[1., 0.], [0., 1.], [1., 0.]])
self.by = np.ones(3)
self.cz = np.ones(2)
self.dz = 1.
self.y = self.prog.NewContinuousVariables(2, "y")
self.z = self.prog.NewContinuousVariables(2, "z")
def test_point_convex_set(self):
p = np.array([11.1, 12.2, 13.3])
point = mut.Point(p)
self.assertEqual(point.ambient_dimension(), 3)
np.testing.assert_array_equal(point.x(), p)
point.set_x(x=2 * p)
np.testing.assert_array_equal(point.x(), 2 * p)
point.set_x(x=p)
assert_pickle(self, point, lambda S: S.x())
# TODO(SeanCurtis-TRI): This doesn't test the constructor that
# builds from shape.
def test_h_polyhedron(self):
hpoly = mut.HPolyhedron(A=self.A, b=self.b)
self.assertEqual(hpoly.ambient_dimension(), 3)
np.testing.assert_array_equal(hpoly.A(), self.A)
np.testing.assert_array_equal(hpoly.b(), self.b)
self.assertTrue(hpoly.PointInSet(x=[0, 0, 0], tol=0.0))
self.assertFalse(hpoly.IsBounded())
hpoly.AddPointInSetConstraints(self.prog, self.x)
constraints = hpoly.AddPointInNonnegativeScalingConstraints(
prog=self.prog, x=self.x, t=self.t)
self.assertGreaterEqual(len(constraints), 2)
self.assertIsInstance(constraints[0], Binding[Constraint])
constraints = hpoly.AddPointInNonnegativeScalingConstraints(
prog=self.prog,
A=self.Ay,
b=self.by,
c=self.cz,
d=self.dz,
x=self.y,
t=self.z)
self.assertGreaterEqual(len(constraints), 2)
self.assertIsInstance(constraints[0], Binding[Constraint])
with self.assertRaisesRegex(RuntimeError,
".*not implemented yet for HPolyhedron.*"):
hpoly.ToShapeWithPose()
assert_pickle(self, hpoly, lambda S: np.vstack((S.A(), S.b())))
h_box = mut.HPolyhedron.MakeBox(lb=[-1, -1, -1], ub=[1, 1, 1])
self.assertTrue(h_box.IntersectsWith(hpoly))
h_unit_box = mut.HPolyhedron.MakeUnitBox(dim=3)
np.testing.assert_array_equal(h_box.A(), h_unit_box.A())
np.testing.assert_array_equal(h_box.b(), h_unit_box.b())
self.assertIsInstance(h_box.MaximumVolumeInscribedEllipsoid(),
mut.Hyperellipsoid)
np.testing.assert_array_almost_equal(h_box.ChebyshevCenter(),
[0, 0, 0])
h2 = h_box.CartesianProduct(other=h_unit_box)
self.assertIsInstance(h2, mut.HPolyhedron)
self.assertEqual(h2.ambient_dimension(), 6)
h3 = h_box.CartesianPower(n=3)
self.assertIsInstance(h3, mut.HPolyhedron)
self.assertEqual(h3.ambient_dimension(), 9)
h4 = h_box.Intersection(other=h_unit_box)
self.assertIsInstance(h4, mut.HPolyhedron)
self.assertEqual(h4.ambient_dimension(), 3)
h5 = h_box.PontryaginDifference(other=h_unit_box)
self.assertIsInstance(h5, mut.HPolyhedron)
np.testing.assert_array_equal(h5.A(), h_box.A())
np.testing.assert_array_equal(h5.b(), np.zeros(6))
def test_hyper_ellipsoid(self):
ellipsoid = mut.Hyperellipsoid(A=self.A, center=self.b)
self.assertEqual(ellipsoid.ambient_dimension(), 3)
np.testing.assert_array_equal(ellipsoid.A(), self.A)
np.testing.assert_array_equal(ellipsoid.center(), self.b)
self.assertTrue(ellipsoid.PointInSet(x=self.b, tol=0.0))
ellipsoid.AddPointInSetConstraints(self.prog, self.x)
constraints = ellipsoid.AddPointInNonnegativeScalingConstraints(
prog=self.prog, x=self.x, t=self.t)
self.assertGreaterEqual(len(constraints), 2)
self.assertIsInstance(constraints[0], Binding[Constraint])
constraints = ellipsoid.AddPointInNonnegativeScalingConstraints(
prog=self.prog,
A=self.Ay,
b=self.by,
c=self.cz,
d=self.dz,
x=self.y,
t=self.z)
self.assertGreaterEqual(len(constraints), 2)
self.assertIsInstance(constraints[0], Binding[Constraint])
shape, pose = ellipsoid.ToShapeWithPose()
self.assertIsInstance(shape, Ellipsoid)
self.assertIsInstance(pose, RigidTransform)
p = np.array([11.1, 12.2, 13.3])
point = mut.Point(p)
scale, witness = ellipsoid.MinimumUniformScalingToTouch(point)
self.assertTrue(scale > 0.0)
np.testing.assert_array_almost_equal(witness, p)
assert_pickle(self, ellipsoid, lambda S: np.vstack(
(S.A(), S.center())))
e_ball = mut.Hyperellipsoid.MakeAxisAligned(radius=[1, 1, 1],
center=self.b)
np.testing.assert_array_equal(e_ball.A(), self.A)
np.testing.assert_array_equal(e_ball.center(), self.b)
e_ball2 = mut.Hyperellipsoid.MakeHypersphere(radius=1, center=self.b)
np.testing.assert_array_equal(e_ball2.A(), self.A)
np.testing.assert_array_equal(e_ball2.center(), self.b)
e_ball3 = mut.Hyperellipsoid.MakeUnitBall(dim=3)
np.testing.assert_array_equal(e_ball3.A(), self.A)
np.testing.assert_array_equal(e_ball3.center(), [0, 0, 0])
def test_minkowski_sum(self):
point = mut.Point(np.array([11.1, 12.2, 13.3]))
hpoly = mut.HPolyhedron(A=self.A, b=self.b)
sum = mut.MinkowskiSum(setA=point, setB=hpoly)
self.assertEqual(sum.ambient_dimension(), 3)
self.assertEqual(sum.num_terms(), 2)
sum2 = mut.MinkowskiSum(sets=[point, hpoly])
self.assertEqual(sum2.ambient_dimension(), 3)
self.assertEqual(sum2.num_terms(), 2)
self.assertIsInstance(sum2.term(0), mut.Point)
def test_v_polytope(self):
vertices = np.array([[0.0, 1.0, 2.0], [3.0, 7.0, 5.0]])
vpoly = mut.VPolytope(vertices=vertices)
self.assertEqual(vpoly.ambient_dimension(), 2)
np.testing.assert_array_equal(vpoly.vertices(), vertices)
self.assertTrue(vpoly.PointInSet(x=[1.0, 5.0], tol=1e-8))
vpoly.AddPointInSetConstraints(self.prog, self.x[0:2])
constraints = vpoly.AddPointInNonnegativeScalingConstraints(
prog=self.prog, x=self.x[:2], t=self.t)
self.assertGreaterEqual(len(constraints), 2)
self.assertIsInstance(constraints[0], Binding[Constraint])
constraints = vpoly.AddPointInNonnegativeScalingConstraints(
prog=self.prog,
A=self.Ay[:2],
b=self.by[:2],
c=self.cz,
d=self.dz,
x=self.y,
t=self.z)
self.assertGreaterEqual(len(constraints), 2)
self.assertIsInstance(constraints[0], Binding[Constraint])
assert_pickle(self, vpoly, lambda S: S.vertices())
v_box = mut.VPolytope.MakeBox(lb=[-1, -1, -1], ub=[1, 1, 1])
self.assertTrue(v_box.PointInSet([0, 0, 0]))
self.assertAlmostEqual(v_box.CalcVolume(), 8, 1E-10)
v_unit_box = mut.VPolytope.MakeUnitBox(dim=3)
self.assertTrue(v_unit_box.PointInSet([0, 0, 0]))
v_from_h = mut.VPolytope(H=mut.HPolyhedron.MakeUnitBox(dim=3))
self.assertTrue(v_from_h.PointInSet([0, 0, 0]))
# Test creating a vpolytope from a non-minimal set of vertices
# 2D: Random points inside a circle
r = 2.0
n = 400
vertices = np.zeros((2, n + 4))
theta = np.linspace(0, 2 * np.pi, n, endpoint=False)
vertices[0, 0:n] = r * np.cos(theta)
vertices[1, 0:n] = r * np.sin(theta)
vertices[:, n:] = np.array([[r / 2, r / 3, r / 4, r / 5],
[r / 2, r / 3, r / 4, r / 5]])
vpoly = mut.VPolytope(vertices=vertices).GetMinimalRepresentation()
self.assertAlmostEqual(vpoly.CalcVolume(), np.pi * r * r, delta=1e-3)
self.assertEqual(vpoly.vertices().shape[1], n)
# Calculate the length of the path that visits all the vertices
# sequentially.
# If the vertices are in clockwise/counter-clockwise order,
# the length of the path will coincide with the perimeter of a
# circle.
self.assertAlmostEqual(self._calculate_path_length(vpoly.vertices()),
2 * np.pi * r,
delta=1e-3)
# 3D: Random points inside a box
a = 2.0
vertices = np.array(
[[0, a, 0, a, 0, a, 0, a, a / 2, a / 3, a / 4, a / 5],
[0, 0, a, a, 0, 0, a, a, a / 2, a / 3, a / 4, a / 5],
[0, 0, 0, 0, a, a, a, a, a / 2, a / 3, a / 4, a / 5]])
vpoly = mut.VPolytope(vertices=vertices).GetMinimalRepresentation()
self.assertAlmostEqual(vpoly.CalcVolume(), a * a * a)
self.assertEqual(vpoly.vertices().shape[1], 8)
def _calculate_path_length(self, vertices):
n = vertices.shape[1]
length = 0
for i in range(n):
j = (i + 1) % n
diff = vertices[:, i] - vertices[:, j]
length += np.sqrt(np.dot(diff, diff))
return length
def test_cartesian_product(self):
point = mut.Point(np.array([11.1, 12.2, 13.3]))
h_box = mut.HPolyhedron.MakeBox(lb=[-1, -1, -1], ub=[1, 1, 1])
sum = mut.CartesianProduct(setA=point, setB=h_box)
self.assertEqual(sum.ambient_dimension(), 6)
self.assertEqual(sum.num_factors(), 2)
sum2 = mut.CartesianProduct(sets=[point, h_box])
self.assertEqual(sum2.ambient_dimension(), 6)
self.assertEqual(sum2.num_factors(), 2)
self.assertIsInstance(sum2.factor(0), mut.Point)
sum2 = mut.CartesianProduct(sets=[point, h_box],
A=np.eye(6, 3),
b=[0, 1, 2, 3, 4, 5])
self.assertEqual(sum2.ambient_dimension(), 3)
self.assertEqual(sum2.num_factors(), 2)
self.assertIsInstance(sum2.factor(1), mut.HPolyhedron)
def test_intersection(self):
point = mut.Point(np.array([0.1, 0.2, 0.3]))
h_box = mut.HPolyhedron.MakeBox(lb=[-1, -1, -1], ub=[1, 1, 1])
intersect = mut.Intersection(setA=point, setB=h_box)
self.assertEqual(intersect.ambient_dimension(), 3)
self.assertEqual(intersect.num_elements(), 2)
intersect2 = mut.Intersection(sets=[point, h_box])
self.assertEqual(intersect2.ambient_dimension(), 3)
self.assertEqual(intersect2.num_elements(), 2)
self.assertIsInstance(intersect2.element(0), mut.Point)
def test_make_from_scene_graph_and_iris(self):
"""
Tests the make from scene graph and iris functionality together as
the Iris code makes obstacles from geometries registered in SceneGraph.
"""
scene_graph = SceneGraph()
source_id = scene_graph.RegisterSource("source")
frame_id = scene_graph.RegisterFrame(source_id=source_id,
frame=GeometryFrame("frame"))
box_geometry_id = scene_graph.RegisterGeometry(
source_id=source_id,
frame_id=frame_id,
geometry=GeometryInstance(X_PG=RigidTransform(),
shape=Box(1., 1., 1.),
name="box"))
cylinder_geometry_id = scene_graph.RegisterGeometry(
source_id=source_id,
frame_id=frame_id,
geometry=GeometryInstance(X_PG=RigidTransform(),
shape=Cylinder(1., 1.),
name="cylinder"))
sphere_geometry_id = scene_graph.RegisterGeometry(
source_id=source_id,
frame_id=frame_id,
geometry=GeometryInstance(X_PG=RigidTransform(),
shape=Sphere(1.),
name="sphere"))
capsule_geometry_id = scene_graph.RegisterGeometry(
source_id=source_id,
frame_id=frame_id,
geometry=GeometryInstance(X_PG=RigidTransform(),
shape=Capsule(1., 1.0),
name="capsule"))
context = scene_graph.CreateDefaultContext()
pose_vector = FramePoseVector()
pose_vector.set_value(frame_id, RigidTransform())
scene_graph.get_source_pose_port(source_id).FixValue(
context, pose_vector)
query_object = scene_graph.get_query_output_port().Eval(context)
H = mut.HPolyhedron(query_object=query_object,
geometry_id=box_geometry_id,
reference_frame=scene_graph.world_frame_id())
self.assertEqual(H.ambient_dimension(), 3)
C = mut.CartesianProduct(query_object=query_object,
geometry_id=cylinder_geometry_id,
reference_frame=scene_graph.world_frame_id())
self.assertEqual(C.ambient_dimension(), 3)
E = mut.Hyperellipsoid(query_object=query_object,
geometry_id=sphere_geometry_id,
reference_frame=scene_graph.world_frame_id())
self.assertEqual(E.ambient_dimension(), 3)
S = mut.MinkowskiSum(query_object=query_object,
geometry_id=capsule_geometry_id,
reference_frame=scene_graph.world_frame_id())
self.assertEqual(S.ambient_dimension(), 3)
P = mut.Point(query_object=query_object,
geometry_id=sphere_geometry_id,
reference_frame=scene_graph.world_frame_id(),
maximum_allowable_radius=1.5)
self.assertEqual(P.ambient_dimension(), 3)
V = mut.VPolytope(query_object=query_object,
geometry_id=box_geometry_id,
reference_frame=scene_graph.world_frame_id())
self.assertEqual(V.ambient_dimension(), 3)
obstacles = mut.MakeIrisObstacles(
query_object=query_object,
reference_frame=scene_graph.world_frame_id())
options = mut.IrisOptions()
options.require_sample_point_is_contained = True
options.iteration_limit = 1
options.termination_threshold = 0.1
options.relative_termination_threshold = 0.01
self.assertNotIn("object at 0x", repr(options))
region = mut.Iris(obstacles=obstacles,
sample=[2, 3.4, 5],
domain=mut.HPolyhedron.MakeBox(lb=[-5, -5, -5],
ub=[5, 5, 5]),
options=options)
self.assertIsInstance(region, mut.HPolyhedron)
obstacles = [
mut.HPolyhedron.MakeUnitBox(3),
mut.Hyperellipsoid.MakeUnitBall(3),
mut.Point([0, 0, 0]),
mut.VPolytope.MakeUnitBox(3)
]
region = mut.Iris(obstacles=obstacles,
sample=[2, 3.4, 5],
domain=mut.HPolyhedron.MakeBox(lb=[-5, -5, -5],
ub=[5, 5, 5]),
options=options)
self.assertIsInstance(region, mut.HPolyhedron)
def test_iris_cspace(self):
limits_urdf = """
<robot name="limits">
<link name="movable">
<collision>
<geometry><box size="1 1 1"/></geometry>
</collision>
</link>
<joint name="movable" type="prismatic">
<axis xyz="1 0 0"/>
<limit lower="-2" upper="2"/>
<parent link="world"/>
<child link="movable"/>
</joint>
</robot>"""
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
Parser(plant).AddModelFromString(limits_urdf, "urdf")
plant.Finalize()
diagram = builder.Build()
context = diagram.CreateDefaultContext()
options = mut.IrisOptions()
plant.SetPositions(plant.GetMyMutableContextFromRoot(context), [0])
region = mut.IrisInConfigurationSpace(
plant=plant,
context=plant.GetMyContextFromRoot(context),
options=options)
self.assertIsInstance(region, mut.ConvexSet)
self.assertEqual(region.ambient_dimension(), 1)
self.assertTrue(region.PointInSet([1.0]))
self.assertFalse(region.PointInSet([3.0]))
def test_graph_of_convex_sets(self):
spp = mut.GraphOfConvexSets()
source = spp.AddVertex(set=mut.Point([0.1]), name="source")
target = spp.AddVertex(set=mut.Point([0.2]), name="target")
edge0 = spp.AddEdge(u=source, v=target, name="edge0")
edge1 = spp.AddEdge(u_id=source.id(), v_id=target.id(), name="edge1")
self.assertEqual(len(spp.Vertices()), 2)
self.assertEqual(len(spp.Edges()), 2)
result = spp.SolveShortestPath(source_id=source.id(),
target_id=target.id(),
convex_relaxation=True)
self.assertIsInstance(result, MathematicalProgramResult)
self.assertIsInstance(
spp.SolveShortestPath(source_id=source.id(),
target_id=target.id(),
convex_relaxation=True,
solver=ClpSolver()),
MathematicalProgramResult)
self.assertIsInstance(
spp.SolveShortestPath(source_id=source.id(),
target_id=target.id(),
convex_relaxation=True,
solver_options=SolverOptions()),
MathematicalProgramResult)
self.assertIsInstance(
spp.SolveShortestPath(source=source,
target=target,
convex_relaxation=True),
MathematicalProgramResult)
self.assertIsInstance(
spp.SolveShortestPath(source=source,
target=target,
convex_relaxation=True,
solver=ClpSolver()),
MathematicalProgramResult)
self.assertIsInstance(
spp.SolveShortestPath(source=source,
target=target,
convex_relaxation=True,
solver_options=SolverOptions()),
MathematicalProgramResult)
self.assertIn(
"source",
spp.GetGraphvizString(result=result,
show_slacks=True,
precision=2,
scientific=False))
# Vertex
self.assertIsInstance(source.id(), mut.GraphOfConvexSets.VertexId)
self.assertEqual(source.ambient_dimension(), 1)
self.assertEqual(source.name(), "source")
self.assertIsInstance(source.x()[0], Variable)
self.assertIsInstance(source.set(), mut.Point)
np.testing.assert_array_almost_equal(source.GetSolution(result), [0.1],
1e-6)
# Edge
self.assertAlmostEqual(edge0.GetSolutionCost(result=result), 0.0, 1e-6)
np.testing.assert_array_almost_equal(
edge0.GetSolutionPhiXu(result=result), [0.1], 1e-6)
np.testing.assert_array_almost_equal(
edge0.GetSolutionPhiXv(result=result), [0.2], 1e-6)
self.assertIsInstance(edge0.id(), mut.GraphOfConvexSets.EdgeId)
self.assertEqual(edge0.name(), "edge0")
self.assertEqual(edge0.u(), source)
self.assertEqual(edge0.v(), target)
self.assertIsInstance(edge0.phi(), Variable)
self.assertIsInstance(edge0.xu()[0], Variable)
self.assertIsInstance(edge0.xv()[0], Variable)
var, binding = edge0.AddCost(e=1.0 + edge0.xu()[0])
self.assertIsInstance(var, Variable)
self.assertIsInstance(binding, Binding[Cost])
var, binding = edge0.AddCost(binding=binding)
self.assertIsInstance(var, Variable)
self.assertIsInstance(binding, Binding[Cost])
self.assertEqual(len(edge0.GetCosts()), 2)
binding = edge0.AddConstraint(f=(edge0.xu()[0] == edge0.xv()[0]))
self.assertIsInstance(binding, Binding[Constraint])
binding = edge0.AddConstraint(binding=binding)
self.assertIsInstance(binding, Binding[Constraint])
self.assertEqual(len(edge0.GetConstraints()), 2)
edge0.AddPhiConstraint(phi_value=False)
edge0.ClearPhiConstraints()
# Remove Edges
self.assertEqual(len(spp.Edges()), 2)
spp.RemoveEdge(edge1.id())
self.assertEqual(len(spp.Edges()), 1)
spp.RemoveEdge(edge0)
self.assertEqual(len(spp.Edges()), 0)
# Remove Vertices
self.assertEqual(len(spp.Vertices()), 2)
spp.RemoveVertex(source.id())
self.assertEqual(len(spp.Vertices()), 1)
spp.RemoveVertex(target)
self.assertEqual(len(spp.Vertices()), 0)
def compute_input(self, x, xd, initial_guess=None, tol=0.0):
prog = MathematicalProgram()
# Joint configuration states & Contact forces
q = prog.NewContinuousVariables(rows=self.T + 1,
cols=self.nq,
name='q')
v = prog.NewContinuousVariables(rows=self.T + 1,
cols=self.nq,
name='v')
u = prog.NewContinuousVariables(rows=self.T, cols=self.nu, name='u')
contact = prog.NewContinuousVariables(rows=self.T,
cols=self.nf,
name='lambda')
#
alpha = prog.NewContinuousVariables(rows=self.T, cols=2, name='alpha')
beta = prog.NewContinuousVariables(rows=self.T, cols=2, name='beta')
# Add Initial Condition Constraint
prog.AddConstraint(eq(q[0], np.array(x[0:3])))
prog.AddConstraint(eq(v[0], np.array(x[3:6])))
# Add Final Condition Constraint
prog.AddConstraint(eq(q[self.T], np.array(xd[0:3])))
prog.AddConstraint(eq(v[self.T], np.array(xd[3:6])))
# Add Dynamics Constraints
for t in range(self.T):
# Add Dynamics Constraints
prog.AddConstraint(
eq(q[t + 1], (q[t] + self.sim.params['h'] * v[t + 1])))
prog.AddConstraint(v[t + 1, 0] == (
v[t, 0] + self.sim.params['h'] *
(-self.sim.params['c'] * v[t, 0] - contact[t, 0] + u[t, 0])))
prog.AddConstraint(v[t + 1,
1] == (v[t, 1] + self.sim.params['h'] *
(-self.sim.params['c'] * v[t, 1] +
contact[t, 0] - contact[t, 1])))
prog.AddConstraint(v[t + 1, 2] == (
v[t, 2] + self.sim.params['h'] *
(-self.sim.params['c'] * v[t, 2] + contact[t, 1] + u[t, 1])))
# Add Contact Constraints
prog.AddConstraint(ge(alpha[t], 0))
prog.AddConstraint(ge(beta[t], 0))
prog.AddConstraint(alpha[t, 0] == contact[t, 0])
prog.AddConstraint(alpha[t, 1] == contact[t, 1])
prog.AddConstraint(
beta[t, 0] == (contact[t, 0] + self.sim.params['k'] *
(q[t, 1] - q[t, 0] - self.sim.params['d'])))
prog.AddConstraint(
beta[t, 1] == (contact[t, 1] + self.sim.params['k'] *
(q[t, 2] - q[t, 1] - self.sim.params['d'])))
# Complementarity constraints. Start with relaxed version and start constraining.
prog.AddConstraint(alpha[t, 0] * beta[t, 0] <= tol)
prog.AddConstraint(alpha[t, 1] * beta[t, 1] <= tol)
# Add Input Constraints and Contact Constraints
prog.AddConstraint(le(contact[t], self.contact_max))
prog.AddConstraint(ge(contact[t], -self.contact_max))
prog.AddConstraint(le(u[t], self.input_max))
prog.AddConstraint(ge(u[t], -self.input_max))
# Add Costs
prog.AddCost(u[t].dot(u[t]))
# Set Initial Guess as empty. Otherwise, start from last solver iteration.
if (type(initial_guess) == type(None)):
initial_guess = np.empty(prog.num_vars())
# Populate initial guess by linearly interpolating between initial
# and final states
#qinit = np.linspace(x[0:3], xd[0:3], self.T + 1)
qinit = np.tile(np.array(x[0:3]), (self.T + 1, 1))
vinit = np.tile(np.array(x[3:6]), (self.T + 1, 1))
uinit = np.tile(np.array([0, 0]), (self.T, 1))
finit = np.tile(np.array([0, 0]), (self.T, 1))
prog.SetDecisionVariableValueInVector(q, qinit, initial_guess)
prog.SetDecisionVariableValueInVector(v, vinit, initial_guess)
prog.SetDecisionVariableValueInVector(u, uinit, initial_guess)
prog.SetDecisionVariableValueInVector(contact, finit,
initial_guess)
# Solve the program
if (self.solver == "ipopt"):
solver = IpoptSolver()
elif (self.solver == "snopt"):
solver = SnoptSolver()
result = solver.Solve(prog, initial_guess)
if (self.solver == "ipopt"):
print("Ipopt Solver Status: ",
result.get_solver_details().status, ", meaning ",
result.get_solver_details().ConvertStatusToString())
elif (self.solver == "snopt"):
val = result.get_solver_details().info
status = self.snopt_status(val)
print("Snopt Solver Status: ",
result.get_solver_details().info, ", meaning ", status)
sol = result.GetSolution()
q_opt = result.GetSolution(q)
v_opt = result.GetSolution(v)
u_opt = result.GetSolution(u)
f_opt = result.GetSolution(contact)
return sol, q_opt, v_opt, u_opt, f_opt
def create_qp1(self, plant_context, V, q_des, v_des, vd_des):
# Determine contact points
contact_positions_per_frame = {}
active_contacts_per_frame = {} # Note this should be in frame space
for frame, contacts in self.contacts_per_frame.items():
contact_positions = self.plant.CalcPointsPositions(
plant_context, self.plant.GetFrameByName(frame), contacts,
self.plant.world_frame())
active_contacts_per_frame[
frame] = contacts[:,
np.where(contact_positions[2, :] <= 0.0)[0]]
N_c = sum([
active_contacts.shape[1]
for active_contacts in active_contacts_per_frame.values()
]) # num contact points
if N_c == 0:
print("Not in contact!")
return None
''' Eq(7) '''
H = self.plant.CalcMassMatrixViaInverseDynamics(plant_context)
# Note that CalcGravityGeneralizedForces assumes the form Mv̇ + C(q, v)v = tau_g(q) + tau_app
# while Eq(7) assumes gravity is accounted in C (on the left hand side)
C_7 = self.plant.CalcBiasTerm(
plant_context) - self.plant.CalcGravityGeneralizedForces(
plant_context)
B_7 = self.B_7
# TODO: Double check
Phi_foots = []
for frame, active_contacts in active_contacts_per_frame.items():
if active_contacts.size:
Phi_foots.append(
self.plant.CalcJacobianTranslationalVelocity(
plant_context, JacobianWrtVariable.kV,
self.plant.GetFrameByName(frame), active_contacts,
self.plant.world_frame(), self.plant.world_frame()))
Phi = np.vstack(Phi_foots)
''' Eq(8) '''
v_idx_act = self.v_idx_act
H_f = H[0:v_idx_act, :]
H_a = H[v_idx_act:, :]
C_f = C_7[0:v_idx_act]
C_a = C_7[v_idx_act:]
B_a = self.B_a
Phi_f_T = Phi.T[0:v_idx_act:, :]
Phi_a_T = Phi.T[v_idx_act:, :]
''' Eq(9) '''
# Assume flat ground for now
n = np.array([[0], [0], [1.0]])
d = np.array([[1.0, -1.0, 0.0, 0.0], [0.0, 0.0, 1.0, -1.0],
[0.0, 0.0, 0.0, 0.0]])
v = np.zeros((N_d, N_c, N_f))
for i in range(N_d):
for j in range(N_c):
v[i, j] = (n + mu * d)[:, i]
''' Quadratic Program I '''
prog = MathematicalProgram()
qdd = prog.NewContinuousVariables(
self.plant.num_velocities(),
name="qdd") # To ignore 6 DOF floating base
self.qdd = qdd
beta = prog.NewContinuousVariables(N_d, N_c, name="beta")
self.beta = beta
lambd = prog.NewContinuousVariables(N_f * N_c, name="lambda")
self.lambd = lambd
# Jacobians ignoring the 6DOF floating base
J_foots = []
for frame, active_contacts in active_contacts_per_frame.items():
if active_contacts.size:
num_active_contacts = active_contacts.shape[1]
J_foot = np.zeros((N_f * num_active_contacts, Atlas.TOTAL_DOF))
# TODO: Can this be simplified?
for i in range(num_active_contacts):
J_foot[N_f * i:N_f *
(i +
1), :] = self.plant.CalcJacobianSpatialVelocity(
plant_context, JacobianWrtVariable.kV,
self.plant.GetFrameByName(frame),
active_contacts[:,
i], self.plant.world_frame(),
self.plant.world_frame())[3:]
J_foots.append(J_foot)
J = np.vstack(J_foots)
assert (J.shape == (N_c * N_f, Atlas.TOTAL_DOF))
eta = prog.NewContinuousVariables(J.shape[0], name="eta")
self.eta = eta
x = prog.NewContinuousVariables(
self.x_size, name="x") # x_com, y_com, x_com_d, y_com_d
self.x = x
u = prog.NewContinuousVariables(self.u_size,
name="u") # x_com_dd, y_com_dd
self.u = u
''' Eq(10) '''
w = 0.01
epsilon = 1.0e-8
K_p = 10.0
K_d = 4.0
frame_weights = np.ones((Atlas.TOTAL_DOF))
q = self.plant.GetPositions(plant_context)
qd = self.plant.GetVelocities(plant_context)
# Convert q, q_nom to generalized velocities form
q_err = self.plant.MapQDotToVelocity(plant_context, q_des - q)
# print(f"Pelvis error: {q_err[0:3]}")
## FIXME: Not sure if it's a good idea to ignore the x, y, z position of pelvis
# ignored_pose_indices = {3, 4, 5} # Ignore x position, y position
ignored_pose_indices = {} # Ignore x position, y position
relevant_pose_indices = list(
set(range(Atlas.TOTAL_DOF)) - set(ignored_pose_indices))
self.relevant_pose_indices = relevant_pose_indices
qdd_ref = K_p * q_err + K_d * (v_des - qd) + vd_des # Eq(27) of [1]
qdd_err = qdd_ref - qdd
qdd_err = qdd_err * frame_weights
qdd_err = qdd_err[relevant_pose_indices]
prog.AddCost(
V(x, u) + w * ((qdd_err).dot(qdd_err)) +
epsilon * np.sum(np.square(beta)) + eta.dot(eta))
''' Eq(11) - 0.003s '''
eq11_lhs = H_f.dot(qdd) + C_f
eq11_rhs = Phi_f_T.dot(lambd)
prog.AddLinearConstraint(eq(eq11_lhs, eq11_rhs))
''' Eq(12) - 0.005s '''
alpha = 0.1
# TODO: Double check
Jd_qd_foots = []
for frame, active_contacts in active_contacts_per_frame.items():
if active_contacts.size:
Jd_qd_foot = self.plant.CalcBiasTranslationalAcceleration(
plant_context, JacobianWrtVariable.kV,
self.plant.GetFrameByName(frame), active_contacts,
self.plant.world_frame(), self.plant.world_frame())
Jd_qd_foots.append(Jd_qd_foot.flatten())
Jd_qd = np.concatenate(Jd_qd_foots)
assert (Jd_qd.shape == (N_c * 3, ))
eq12_lhs = J.dot(qdd) + Jd_qd
eq12_rhs = -alpha * J.dot(qd) + eta
prog.AddLinearConstraint(eq(eq12_lhs, eq12_rhs))
''' Eq(13) - 0.015s '''
def tau(qdd, lambd):
return self.B_a_inv.dot(H_a.dot(qdd) + C_a - Phi_a_T.dot(lambd))
self.tau = tau
eq13_lhs = self.tau(qdd, lambd)
prog.AddLinearConstraint(ge(eq13_lhs, -self.sorted_max_efforts))
prog.AddLinearConstraint(le(eq13_lhs, self.sorted_max_efforts))
''' Eq(14) '''
for j in range(N_c):
beta_v = beta[:, j].dot(v[:, j])
prog.AddLinearConstraint(eq(lambd[N_f * j:N_f * j + 3], beta_v))
''' Eq(15) '''
for b in beta.flat:
prog.AddLinearConstraint(b >= 0.0)
''' Eq(16) '''
prog.AddLinearConstraint(ge(eta, eta_min))
prog.AddLinearConstraint(le(eta, eta_max))
''' Enforce x as com '''
com = self.plant.CalcCenterOfMassPosition(plant_context)
com_d = self.plant.CalcJacobianCenterOfMassTranslationalVelocity(
plant_context, JacobianWrtVariable.kV, self.plant.world_frame(),
self.plant.world_frame()).dot(qd)
prog.AddLinearConstraint(x[0] == com[0])
prog.AddLinearConstraint(x[1] == com[1])
prog.AddLinearConstraint(x[2] == com_d[0])
prog.AddLinearConstraint(x[3] == com_d[1])
''' Enforce u as com_dd '''
com_dd = (
self.plant.CalcBiasCenterOfMassTranslationalAcceleration(
plant_context, JacobianWrtVariable.kV,
self.plant.world_frame(), self.plant.world_frame()) +
self.plant.CalcJacobianCenterOfMassTranslationalVelocity(
plant_context, JacobianWrtVariable.kV,
self.plant.world_frame(), self.plant.world_frame()).dot(qdd))
prog.AddLinearConstraint(u[0] == com_dd[0])
prog.AddLinearConstraint(u[1] == com_dd[1])
''' Respect joint limits '''
for name, limit in Atlas.JOINT_LIMITS.items():
# Get the corresponding joint value
joint_pos = self.plant.GetJointByName(name).get_angle(
plant_context)
# Get the corresponding actuator index
act_idx = getActuatorIndex(self.plant, name)
# Use the actuator index to find the corresponding generalized coordinate index
# q_idx = np.where(B_7[:,act_idx] == 1)[0][0]
q_idx = getJointIndexInGeneralizedVelocities(self.plant, name)
if joint_pos >= limit.upper:
# print(f"Joint {name} max reached")
prog.AddLinearConstraint(qdd[q_idx] <= 0.0)
elif joint_pos <= limit.lower:
# print(f"Joint {name} min reached")
prog.AddLinearConstraint(qdd[q_idx] >= 0.0)
return prog
"""
Solve an optimization problem
min x(0)^2 + x(1)^2
s.t. x(0) + x(1) = 1
x(0) <= x(1)
this example also demonstrates how to add a callback to the program, and use it to visualize the progression of the solver.
Adapted from the MathematicalProgram tutorial on the Drake website: https://drake.mit.edu
"""
from pydrake.solvers.mathematicalprogram import MathematicalProgram, Solve
import numpy as np
import matplotlib.pyplot as plt
# Create empty MathematicalProgram
prog = MathematicalProgram()
# Add 2 continuous decision variables
# x is a numpy array - we can use an optional second argument to name the variables
x = prog.NewContinuousVariables(2)
#print(x)
prog.AddConstraint(x[0] + x[1] == 1)
prog.AddConstraint(x[0] <= x[1])
prog.AddCost(x[0]**2 + x[1]**2)
# Make and add a visualization callback
fig = plt.figure()
curve_x = np.linspace(1, 10, 100)
ax = plt.gca()
ax.plot(curve_x, 9. / curve_x)
ax.plot(-curve_x, -9. / curve_x)
plant = plant_ad
context = context_ad
# Do forward kinematics
plant.SetPositions(context, iiwa, q)
X_WL7 = plant.CalcRelativeTransform(context, resolve_frame(plant, W),
resolve_frame(plant, L7))
p_TL7 = X_WL7.translation() - p_WT
# WARNING: If you return a scalar for a constraint, or a vector for
# a cost, you may get the following cryptic error:
# "Unable to cast Python instance to c++ type"
link_7_distance_to_target_vector = lambda q: [link_7_distance_to_target(q)]
# New program: Using a custom evaluator
prog = MathematicalProgram()
q = prog.NewContinuousVariables(plant_f.num_positions())
# Define nominal configuration
q0 = np.zeros(plant_f.num_positions())
# Add basic cost. (This will be parsed into a quadratic cost)
prog.AddCost((q - q0).dot(q - q0))
# Add constraint based on custom evaluator.
prog.AddConstraint(link_7_distance_to_target_vector,
lb=[0.1],
ub=[0.2],
vars=q)
result = Solve(prog, initial_guess=q0)
from pydrake.solvers.mathematicalprogram import MathematicalProgram, Solve
import numpy as np
# Creat an empty MathematicalProgram named prog (with no decision variables, constraints or costs)
prog = MathematicalProgram()
# Add two decision variables x[0], x[1]
x = prog.NewContinuousVariables(2, "x")
# Add a symbolic linear expression as the cost
cost_1 = prog.AddLinearCost(x[0] + 3 * x[1] + 2)
# Print the newly added cost
print(cost_1)
# The newly added cost is stored in prog.linear_costs()
print(prog.linear_costs()[0])
cost_2 = prog.AddLinearCost(2 * x[1] + 3)
print(f"number of linear cost objects: {len(prog.linear_costs())}")
# Can also add costs in a vector coefficient form
cost_3 = prog.AddLinearCost([3., 4.], 5.0, x)
print(cost_3)
# Can also just call the generic "AddCost".
# If drake thinks its linear, gets added to linear costs list
cost_4 = prog.AddCost(x[0] + 3 * x[1] + 5)
print(f"Number of linear cost objects after calling AddCost: {len(prog.linear_costs())}")
# New program, now with constraints
prog = MathematicalProgram()
float_calls+=1
else:
plant = plant_d
context = context_d
autodiff_calls+=1
# Do forward kinematics
plant.SetPositions(context, iiwa, q)
X_WL7 = plant.CalcRelativeTransform(context, resolve_frame(plant, W), resolve_frame(plant, L7))
p_TL7 = X_WL7.translation() - p_WT
# Return scalar squared distance
return p_TL7.dot(p_TL7)
# NOTE: Returning a scalar for a constraint or a vector for a cost could result in cryptic errors
# Create a mathematical program
prog = MathematicalProgram()
q = prog.NewContinuousVariables(plant_f.num_positions())
# define nominal configuration
q0 = np.zeros(plant_f.num_positions())
# Add the custom cost
prog.AddCost(link_7_distance_to_target, vars=q, description="IK_Cost")
# Solve the problem
CheckProgram(prog)
print('Start optimization')
result = Solve(prog, initial_guess=q0)
# Number of autodiff and float calls during optimization
print(f"Number float evaluations: {float_calls}")
print(f"Number of autodiff evaluations: {autodiff_calls}")
print(f"Success? {result.is_success()}")
print(result.get_solution_result())
