def test_kuka(self):
"""Kuka IIWA with mesh geometry."""
file_name = FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/sdf/"
"iiwa14_no_collision.sdf")
builder = DiagramBuilder()
kuka, scene_graph = AddMultibodyPlantSceneGraph(builder)
Parser(plant=kuka).AddModelFromFile(file_name)
kuka.AddForceElement(UniformGravityFieldElement())
kuka.Finalize()
visualizer = builder.AddSystem(
MeshcatVisualizer(scene_graph, zmq_url=None, open_browser=False))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
kuka_context = diagram.GetMutableSubsystemContext(
kuka, diagram_context)
kuka_context.FixInputPort(
kuka.get_actuation_input_port().get_index(),
np.zeros(kuka.get_actuation_input_port().size()))
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.StepTo(.1)
def test_multibody_dynamics(self):
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant()
Parser(plant).AddModelFromFile(file_name)
# Getting ready for when we set foot on Mars :-).
plant.AddForceElement(UniformGravityFieldElement([0.0, 0.0, -3.71]))
plant.Finalize()
context = plant.CreateDefaultContext()
# Set an arbitrary configuration away from the model's fixed point.
plant.SetPositions(context, [0.1, 0.2])
H = plant.CalcMassMatrixViaInverseDynamics(context)
Cv = plant.CalcBiasTerm(context)
self.assertTrue(H.shape == (2, 2))
self.assert_sane(H)
self.assertTrue(Cv.shape == (2, ))
self.assert_sane(Cv, nonzero=False)
nv = plant.num_velocities()
vd_d = np.zeros(nv)
tau = plant.CalcInverseDynamics(context, vd_d, MultibodyForces(plant))
self.assertEqual(tau.shape, (2, ))
self.assert_sane(tau, nonzero=False)
# - Existence checks.
# Gravity leads to non-zero potential energy.
self.assertNotEqual(plant.CalcPotentialEnergy(context), 0)
plant.CalcConservativePower(context)
tau_g = plant.CalcGravityGeneralizedForces(context)
self.assertEqual(tau_g.shape, (nv, ))
self.assert_sane(tau_g, nonzero=True)
forces = MultibodyForces(plant=plant)
plant.CalcForceElementsContribution(context=context, forces=forces)
def test_cart_pole(self):
"""Cart-Pole with simple geometry."""
file_name = FindResourceOrThrow(
"drake/examples/multibody/cart_pole/cart_pole.sdf")
builder = DiagramBuilder()
cart_pole, scene_graph = AddMultibodyPlantSceneGraph(builder)
Parser(plant=cart_pole).AddModelFromFile(file_name)
cart_pole.AddForceElement(UniformGravityFieldElement())
cart_pole.Finalize()
assert cart_pole.geometry_source_is_registered()
visualizer = builder.AddSystem(
MeshcatVisualizer(scene_graph, zmq_url=None, open_browser=False))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
cart_pole_context = diagram.GetMutableSubsystemContext(
cart_pole, diagram_context)
cart_pole_context.FixInputPort(
cart_pole.get_actuation_input_port().get_index(), [0])
cart_slider = cart_pole.GetJointByName("CartSlider")
pole_pin = cart_pole.GetJointByName("PolePin")
cart_slider.set_translation(context=cart_pole_context, translation=0.)
pole_pin.set_angle(context=cart_pole_context, angle=2.)
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.StepTo(.1)
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"--target_realtime_rate",
type=float,
default=1.0,
help="Desired rate relative to real time. See documentation for "
"Simulator::set_target_realtime_rate() for details.")
parser.add_argument("--simulation_time",
type=float,
default=10.0,
help="Desired duration of the simulation in seconds.")
parser.add_argument(
"--time_step",
type=float,
default=0.,
help="If greater than zero, the plant is modeled as a system with "
"discrete updates and period equal to this time_step. "
"If 0, the plant is modeled as a continuous system.")
args = parser.parse_args()
file_name = FindResourceOrThrow(
"drake/examples/multibody/cart_pole/cart_pole.sdf")
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
cart_pole = builder.AddSystem(MultibodyPlant(time_step=args.time_step))
cart_pole.RegisterAsSourceForSceneGraph(scene_graph)
Parser(plant=cart_pole).AddModelFromFile(file_name)
cart_pole.AddForceElement(UniformGravityFieldElement())
cart_pole.Finalize()
assert cart_pole.geometry_source_is_registered()
builder.Connect(scene_graph.get_query_output_port(),
cart_pole.get_geometry_query_input_port())
builder.Connect(
cart_pole.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(cart_pole.get_source_id()))
ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph)
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
cart_pole_context = diagram.GetMutableSubsystemContext(
cart_pole, diagram_context)
cart_pole_context.FixInputPort(
cart_pole.get_actuation_input_port().get_index(), [0])
cart_slider = cart_pole.GetJointByName("CartSlider")
pole_pin = cart_pole.GetJointByName("PolePin")
cart_slider.set_translation(context=cart_pole_context, translation=0.)
pole_pin.set_angle(context=cart_pole_context, angle=2.)
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(args.target_realtime_rate)
simulator.Initialize()
simulator.StepTo(args.simulation_time)
def test_multibody_gravity_default(self):
plant = MultibodyPlant()
# Smoke test of deprecated methods.
with catch_drake_warnings(expected_count=1):
plant.AddForceElement(UniformGravityFieldElement())
plant.Finalize()
def test_multibody_dynamics(self):
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant()
Parser(plant).AddModelFromFile(file_name)
# Getting ready for when we set foot on Mars :-).
plant.AddForceElement(UniformGravityFieldElement([0.0, 0.0, -3.71]))
plant.Finalize()
context = plant.CreateDefaultContext()
# Set an arbitrary configuration away from the model's fixed point.
plant.SetPositions(context, [0.1, 0.2])
M = plant.CalcMassMatrixViaInverseDynamics(context)
Cv = plant.CalcBiasTerm(context)
self.assertTrue(M.shape == (2, 2))
self.assert_sane(M)
self.assertTrue(Cv.shape == (2, ))
self.assert_sane(Cv, nonzero=False)
nv = plant.num_velocities()
vd_d = np.zeros(nv)
tau = plant.CalcInverseDynamics(context, vd_d, MultibodyForces(plant))
self.assertEqual(tau.shape, (2, ))
self.assert_sane(tau, nonzero=False)
# - Existence checks.
# Gravity leads to non-zero potential energy.
self.assertNotEqual(plant.CalcPotentialEnergy(context), 0)
plant.CalcConservativePower(context)
tau_g = plant.CalcGravityGeneralizedForces(context)
self.assertEqual(tau_g.shape, (nv, ))
self.assert_sane(tau_g, nonzero=True)
B = plant.MakeActuationMatrix()
np.testing.assert_equal(B, np.array([[0.], [1.]]))
forces = MultibodyForces(plant=plant)
plant.CalcForceElementsContribution(context=context, forces=forces)
# Test generalized forces.
forces.mutable_generalized_forces()[:] = 1
np.testing.assert_equal(forces.generalized_forces(), 1)
forces.SetZero()
np.testing.assert_equal(forces.generalized_forces(), 0)
# Test body force accessors and mutators.
link2 = plant.GetBodyByName("Link2")
self.assertIsInstance(link2.GetForceInWorld(context, forces),
SpatialForce)
forces.SetZero()
F_expected = np.array([1, 2, 3, 4, 5, 6])
link2.AddInForceInWorld(context,
F_Bo_W=SpatialForce(F=F_expected),
forces=forces)
np.testing.assert_equal(
link2.GetForceInWorld(context, forces).get_coeffs(), F_expected)
link2.AddInForce(context,
p_BP_E=[0, 0, 0],
F_Bp_E=SpatialForce(F=F_expected),
frame_E=plant.world_frame(),
forces=forces)
# Also check accumulation.
np.testing.assert_equal(
link2.GetForceInWorld(context, forces).get_coeffs(),
2 * F_expected)
def test_multibody_gravity_vector(self):
plant = MultibodyPlant()
plant.AddForceElement(UniformGravityFieldElement([0.0, -9.81, 0.0]))
plant.Finalize()
def test_multibody_gravity_default(self):
plant = MultibodyPlant()
plant.AddForceElement(UniformGravityFieldElement())
plant.Finalize()
parser = argparse.ArgumentParser()
parser.add_argument("--test",
action='store_true',
help="Causes the script to run without blocking for "
"user input.",
default=False)
args = parser.parse_args()
file_name = FindResourceOrThrow(
"drake/examples/multibody/cart_pole/cart_pole.sdf")
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
cart_pole = builder.AddSystem(MultibodyPlant())
cart_pole.RegisterAsSourceForSceneGraph(scene_graph)
Parser(plant=cart_pole).AddModelFromFile(file_name)
cart_pole.AddForceElement(UniformGravityFieldElement())
cart_pole.Finalize()
assert cart_pole.geometry_source_is_registered()
builder.Connect(
scene_graph.get_query_output_port(),
cart_pole.get_geometry_query_input_port())
builder.Connect(
cart_pole.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(cart_pole.get_source_id()))
builder.ExportInput(cart_pole.get_actuation_input_port())
ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph)
diagram = builder.Build()
diagram.set_name("graphviz_example")
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"--num_samples",
type=int,
default=50,
help="Number of Monte Carlo samples to use to estimate performance.")
parser.add_argument("--torque_limit",
type=float,
default=2.0,
help="Torque limit of the pendulum.")
args = parser.parse_args()
if args.torque_limit < 0:
raise InvalidArgumentError("Please supply a nonnegative torque limit.")
# Assemble the Pendulum plant.
builder = DiagramBuilder()
pendulum = builder.AddSystem(MultibodyPlant())
file_name = FindResourceOrThrow("drake/examples/pendulum/Pendulum.urdf")
Parser(pendulum).AddModelFromFile(file_name)
pendulum.AddForceElement(UniformGravityFieldElement())
pendulum.WeldFrames(pendulum.world_frame(),
pendulum.GetFrameByName("base_part2"))
pendulum.Finalize()
# Set the pendulum to start at uniformly random
# positions (but always zero velocity).
elbow = pendulum.GetMutableJointByName("theta")
upright_theta = np.pi
theta_expression = Variable(name="theta",
type=Variable.Type.RANDOM_UNIFORM) * 2. * np.pi
elbow.set_random_angle_distribution(theta_expression)
# Set up LQR, with high position gains to try to ensure the
# ROA is close to the theoretical torque-limited limit.
Q = np.diag([100., 1.])
R = np.identity(1) * 0.01
linearize_context = pendulum.CreateDefaultContext()
linearize_context.SetContinuousState(np.array([upright_theta, 0.]))
actuation_port_index = pendulum.get_actuation_input_port().get_index()
linearize_context.FixInputPort(actuation_port_index, np.zeros(1))
controller = builder.AddSystem(
LinearQuadraticRegulator(pendulum, linearize_context, Q, R,
np.zeros(0), actuation_port_index))
# Apply the torque limit.
torque_limit = args.torque_limit
torque_limiter = builder.AddSystem(
Saturation(min_value=np.array([-torque_limit]),
max_value=np.array([torque_limit])))
builder.Connect(controller.get_output_port(0),
torque_limiter.get_input_port(0))
builder.Connect(torque_limiter.get_output_port(0),
pendulum.get_actuation_input_port())
builder.Connect(pendulum.get_output_port(0), controller.get_input_port(0))
diagram = builder.Build()
# Perform the Monte Carlo simulation.
def make_simulator(generator):
''' Create a simulator for the system
using the given generator. '''
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(0)
simulator.Initialize()
return simulator
def calc_wrapped_error(system, context):
''' Given a context from the end of the simulation,
calculate an error -- which for this stabilizing
task is the distance from the
fixed point. '''
state = diagram.GetSubsystemContext(
pendulum, context).get_continuous_state_vector()
error = state.GetAtIndex(0) - upright_theta
# Wrap error to [-pi, pi].
return (error + np.pi) % (2 * np.pi) - np.pi
num_samples = args.num_samples
results = MonteCarloSimulation(make_simulator=make_simulator,
output=calc_wrapped_error,
final_time=1.0,
num_samples=num_samples,
generator=RandomGenerator())
# Compute results.
# The "success" region is fairly large since some "stabilized" trials
# may still be oscillating around the fixed point. Failed examples are
# consistently much farther from the fixed point than this.
binary_results = np.array([abs(res.output) < 0.1 for res in results])
passing_ratio = float(sum(binary_results)) / len(results)
# 95% confidence interval for the passing ratio.
passing_ratio_var = 1.96 * np.sqrt(passing_ratio *
(1. - passing_ratio) / len(results))
print("Monte-Carlo estimated performance across %d samples: "
"%.2f%% +/- %0.2f%%" %
(num_samples, passing_ratio * 100, passing_ratio_var * 100))
# Analytically compute the best possible ROA, for comparison, but
# calculating where the torque needed to lift the pendulum exceeds
# the torque limit.
arm_radius = 0.5
arm_mass = 1.0
# torque = r x f = r * (m * 9.81 * sin(theta))
# theta = asin(torque / (r * m))
if torque_limit <= (arm_radius * arm_mass * 9.81):
roa_half_width = np.arcsin(torque_limit /
(arm_radius * arm_mass * 9.81))
else:
roa_half_width = np.pi
roa_as_fraction_of_state_space = roa_half_width / np.pi
print("Max possible ROA = %0.2f%% of state space, which should"
" match with the above estimate." %
(100 * roa_as_fraction_of_state_space))
def test_contact_force(self):
"""A block sitting on a table."""
object_file_path = FindResourceOrThrow(
"drake/examples/manipulation_station/models/061_foam_brick.sdf")
table_file_path = FindResourceOrThrow(
"drake/examples/kuka_iiwa_arm/models/table/"
"extra_heavy_duty_table_surface_only_collision.sdf")
# T: tabletop frame.
X_TObject = Isometry3.Identity()
X_TObject.set_translation([0, 0, 0.2])
builder = DiagramBuilder()
plant = MultibodyPlant(0.002)
_, scene_graph = AddMultibodyPlantSceneGraph(builder, plant)
object_model = Parser(plant=plant).AddModelFromFile(object_file_path)
table_model = Parser(plant=plant).AddModelFromFile(table_file_path)
# Weld table to world.
plant.WeldFrames(A=plant.world_frame(),
B=plant.GetFrameByName("link", table_model))
plant.AddForceElement(UniformGravityFieldElement())
plant.Finalize()
# Add meshcat visualizer.
viz = builder.AddSystem(
MeshcatVisualizer(scene_graph, zmq_url=None, open_browser=False))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
viz.get_input_port(0))
# Add contact visualizer.
contact_viz = builder.AddSystem(
MeshcatContactVisualizer(meshcat_viz=viz,
force_threshold=0,
contact_force_scale=10,
plant=plant))
contact_input_port = contact_viz.GetInputPort("contact_results")
builder.Connect(plant.GetOutputPort("contact_results"),
contact_input_port)
builder.Connect(scene_graph.get_pose_bundle_output_port(),
contact_viz.GetInputPort("pose_bundle"))
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
mbp_context = diagram.GetMutableSubsystemContext(
plant, diagram_context)
X_WT = plant.CalcRelativeTransform(mbp_context, plant.world_frame(),
plant.GetFrameByName("top_center"))
plant.SetFreeBodyPose(mbp_context,
plant.GetBodyByName("base_link", object_model),
X_WT.multiply(X_TObject))
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.StepTo(1.0)
contact_viz_context = (diagram.GetMutableSubsystemContext(
contact_viz, diagram_context))
contact_results = contact_viz.EvalAbstractInput(
contact_viz_context, contact_input_port.get_index()).get_value()
self.assertGreater(contact_results.num_contacts(), 0)
self.assertEqual(contact_viz._contact_key_counter, 4)
def do_main():
rospy.init_node('run_dishrack_interaction', anonymous=False)
#np.random.seed(42)
for outer_iter in range(200):
try:
builder = DiagramBuilder()
mbp, scene_graph = AddMultibodyPlantSceneGraph(
builder, MultibodyPlant(time_step=0.0025))
# Add ground
world_body = mbp.world_body()
ground_shape = Box(2., 2., 2.)
ground_body = mbp.AddRigidBody(
"ground",
SpatialInertia(mass=10.0,
p_PScm_E=np.array([0., 0., 0.]),
G_SP_E=UnitInertia(1.0, 1.0, 1.0)))
mbp.WeldFrames(world_body.body_frame(), ground_body.body_frame(),
RigidTransform(p=[0, 0, -1]))
mbp.RegisterVisualGeometry(ground_body, RigidTransform.Identity(),
ground_shape, "ground_vis",
np.array([0.5, 0.5, 0.5, 1.]))
mbp.RegisterCollisionGeometry(ground_body,
RigidTransform.Identity(),
ground_shape, "ground_col",
CoulombFriction(0.9, 0.8))
parser = Parser(mbp, scene_graph)
dish_bin_model = "/home/gizatt/projects/scene_generation/models/dish_models/bus_tub_01_decomp/bus_tub_01_decomp.urdf"
cupboard_model = "/home/gizatt/projects/scene_generation/models/dish_models/shelf_two_levels.sdf"
candidate_model_files = {
#"mug": "/home/gizatt/drake/manipulation/models/mug/mug.urdf",
"mug_1":
"/home/gizatt/projects/scene_generation/models/dish_models/mug_1_decomp/mug_1_decomp.urdf",
#"plate_11in": "/home/gizatt/drake/manipulation/models/dish_models/plate_11in_decomp/plate_11in_decomp.urdf",
#"/home/gizatt/drake/manipulation/models/mug_big/mug_big.urdf",
#"/home/gizatt/drake/manipulation/models/dish_models/bowl_6p25in_decomp/bowl_6p25in_decomp.urdf",
#"/home/gizatt/drake/manipulation/models/dish_models/plate_8p5in_decomp/plate_8p5in_decomp.urdf",
}
# Decide how many of each object to add
max_num_objs = 6
num_objs = [
np.random.randint(0, max_num_objs)
for k in range(len(candidate_model_files.keys()))
]
# Actually produce their initial poses + add them to the sim
poses = [] # [quat, pos]
all_object_instances = []
all_manipulable_body_ids = []
total_num_objs = sum(num_objs)
object_ordering = list(range(total_num_objs))
k = 0
random.shuffle(object_ordering)
print("ordering: ", object_ordering)
for class_k, class_entry in enumerate(
candidate_model_files.items()):
for model_instance_k in range(num_objs[class_k]):
class_name, class_path = class_entry
model_name = "%s_%d" % (class_name, model_instance_k)
all_object_instances.append([class_name, model_name])
model_id = parser.AddModelFromFile(class_path,
model_name=model_name)
all_manipulable_body_ids += mbp.GetBodyIndices(model_id)
# Put them in a randomly ordered line, for placing
y_offset = (object_ordering[k] / float(total_num_objs) -
0.5) # RAnge -0.5 to 0.5
poses.append([
RollPitchYaw(np.random.uniform(
0., 2. * np.pi, size=3)).ToQuaternion().wxyz(),
[-0.25, y_offset, 0.1]
])
k += 1
#$poses.append([
# RollPitchYaw(np.random.uniform(0., 2.*np.pi, size=3)).ToQuaternion().wxyz(),
# [np.random.uniform(-0.2, 0.2), np.random.uniform(-0.1, 0.1), np.random.uniform(0.1, 0.3)]])
# Build a desk
parser.AddModelFromFile(cupboard_model)
mbp.WeldFrames(world_body.body_frame(),
mbp.GetBodyByName("shelf_origin_body").body_frame(),
RigidTransform(p=[0.0, 0, 0.0]))
#parser.AddModelFromFile(dish_bin_model)
#mbp.WeldFrames(world_body.body_frame(), mbp.GetBodyByName("bus_tub_01_decomp_body_link").body_frame(),
# RigidTransform(p=[0.0, 0., 0.], rpy=RollPitchYaw(np.pi/2., 0., 0.)))
mbp.AddForceElement(UniformGravityFieldElement())
mbp.Finalize()
hydra_sg_spy = builder.AddSystem(
HydraInteractionLeafSystem(
mbp,
scene_graph,
all_manipulable_body_ids=all_manipulable_body_ids))
#builder.Connect(scene_graph.get_query_output_port(),
# hydra_sg_spy.get_input_port(0))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
hydra_sg_spy.get_input_port(0))
builder.Connect(mbp.get_state_output_port(),
hydra_sg_spy.get_input_port(1))
builder.Connect(hydra_sg_spy.get_output_port(0),
mbp.get_applied_spatial_force_input_port())
visualizer = builder.AddSystem(
MeshcatVisualizer(scene_graph,
zmq_url="tcp://127.0.0.1:6000",
draw_period=0.01))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
mbp_context = diagram.GetMutableSubsystemContext(
mbp, diagram_context)
sg_context = diagram.GetMutableSubsystemContext(
scene_graph, diagram_context)
q0 = mbp.GetPositions(mbp_context).copy()
for k in range(len(poses)):
offset = k * 7
q0[(offset):(offset + 4)] = poses[k][0]
q0[(offset + 4):(offset + 7)] = poses[k][1]
mbp.SetPositions(mbp_context, q0)
simulator = Simulator(diagram, diagram_context)
simulator.set_target_realtime_rate(1.0)
simulator.set_publish_every_time_step(False)
simulator.Initialize()
ik = InverseKinematics(mbp, mbp_context)
q_dec = ik.q()
prog = ik.prog()
def squaredNorm(x):
return np.array([x[0]**2 + x[1]**2 + x[2]**2 + x[3]**2])
for k in range(len(poses)):
# Quaternion norm
prog.AddConstraint(squaredNorm, [1], [1],
q_dec[(k * 7):(k * 7 + 4)])
# Trivial quaternion bounds
prog.AddBoundingBoxConstraint(-np.ones(4), np.ones(4),
q_dec[(k * 7):(k * 7 + 4)])
# Conservative bounds on on XYZ
prog.AddBoundingBoxConstraint(np.array([-2., -2., -2.]),
np.array([2., 2., 2.]),
q_dec[(k * 7 + 4):(k * 7 + 7)])
def vis_callback(x):
vis_diagram_context = diagram.CreateDefaultContext()
mbp.SetPositions(
diagram.GetMutableSubsystemContext(mbp,
vis_diagram_context), x)
pose_bundle = scene_graph.get_pose_bundle_output_port().Eval(
diagram.GetMutableSubsystemContext(scene_graph,
vis_diagram_context))
context = visualizer.CreateDefaultContext()
context.FixInputPort(0, AbstractValue.Make(pose_bundle))
visualizer.Publish(context)
prog.AddVisualizationCallback(vis_callback, q_dec)
prog.AddQuadraticErrorCost(np.eye(q0.shape[0]) * 1.0, q0, q_dec)
ik.AddMinimumDistanceConstraint(0.001, threshold_distance=1.0)
prog.SetInitialGuess(q_dec, q0)
print("Solving")
# print "Initial guess: ", q0
start_time = time.time()
solver = SnoptSolver()
#solver = NloptSolver()
sid = solver.solver_type()
# SNOPT
prog.SetSolverOption(sid, "Print file", "test.snopt")
prog.SetSolverOption(sid, "Major feasibility tolerance", 1e-3)
prog.SetSolverOption(sid, "Major optimality tolerance", 1e-2)
prog.SetSolverOption(sid, "Minor feasibility tolerance", 1e-3)
prog.SetSolverOption(sid, "Scale option", 0)
#prog.SetSolverOption(sid, "Elastic weight", 1e1)
#prog.SetSolverOption(sid, "Elastic mode", "Yes")
# NLOPT
#prog.SetSolverOption(sid, "initial_step", 0.1)
#prog.SetSolverOption(sid, "xtol_rel", 1E-2)
#prog.SetSolverOption(sid, "xtol_abs", 1E-2)
#prog.SetSolverOption(sid, "Major step limit", 2)
print("Solver opts: ", prog.GetSolverOptions(solver.solver_type()))
result = mp.Solve(prog)
print("Solve info: ", result)
print("Solved in %f seconds" % (time.time() - start_time))
#print(IpoptSolver().Solve(prog))
print(result.get_solver_id().name())
q0_proj = result.GetSolution(q_dec)
# print "Final: ", q0_proj
mbp.SetPositions(mbp_context, q0_proj)
simulator.StepTo(1000)
raise StopIteration()
except StopIteration:
print(colored("Stopped, saving and restarting", 'yellow'))
qf = mbp.GetPositions(mbp_context)
# Decide whether to accept: all objs within bounds
save = True
for k in range(len(all_object_instances)):
offset = k * 7
q = qf[offset:(offset + 7)]
if q[4] <= -0.25 or q[4] >= 0.25 or q[5] <= -0.2 or q[
5] >= 0.2 or q[6] <= -0.1:
save = False
break
if save:
print(colored("Saving", "green"))
save_config(all_object_instances, qf,
"mug_rack_environments_human.yaml")
else:
print(
colored("Not saving due to bounds violation: " + str(q),
"yellow"))
except Exception as e:
print(colored("Suffered other exception " + str(e), "red"))
sys.exit(-1)
except:
print(
colored("Suffered totally unknown exception! Probably sim.",
"red"))