def BuildHand(num_fingers=3, manipuland_sdf="models/manipuland_box.sdf"):
''' Build up the hand by replicating a finger
model at a handful of base positions. '''
finger_base_positions = np.vstack([
np.abs(np.linspace(-0.25, 0.25, num_fingers)),
np.linspace(0.25, -0.25, num_fingers),
np.zeros(num_fingers)
]).T
tree = RigidBodyTree()
for i, base_pos in enumerate(finger_base_positions):
frame = RigidBodyFrame(name="finger_%d_base_frame" % i,
body=tree.world(),
xyz=base_pos,
rpy=[0, 0, 0])
tree.addFrame(frame)
AddModelInstancesFromSdfString(
open("models/planar_finger.sdf", 'r').read(),
FloatingBaseType.kFixed, frame, tree)
# Add the manipuland as well from an sdf.
manipuland_frame = RigidBodyFrame(name="manipuland_frame",
body=tree.world(),
xyz=[0, 0., 0.],
rpy=[0, 0, 0])
tree.addFrame(manipuland_frame)
AddModelInstancesFromSdfString(
open(manipuland_sdf, 'r').read(), FloatingBaseType.kFixed,
manipuland_frame, tree)
return tree
def add_model_wrapper(self, filename, floating_base_type, frame, rbt):
if filename.split(".")[-1] == "sdf":
model_instance_map = AddModelInstancesFromSdfString(
open(filename).read(), floating_base_type, frame, rbt)
else:
model_instance_map = AddModelInstanceFromUrdfFile(
filename, floating_base_type, frame, rbt)
for key in model_instance_map.keys():
self.model_index_dict[key] = model_instance_map[key]
def setup_kuka(rbt):
iiwa_urdf_path = os.path.join(pydrake.getDrakePath(), "manipulation",
"models", "iiwa_description", "urdf",
"iiwa14_polytope_collision.urdf")
wsg50_sdf_path = os.path.join(pydrake.getDrakePath(), "manipulation",
"models", "wsg_50_description", "sdf",
"schunk_wsg_50.sdf")
table_sdf_path = os.path.join(
pydrake.getDrakePath(), "examples", "kuka_iiwa_arm", "models", "table",
"extra_heavy_duty_table_surface_only_collision.sdf")
object_urdf_path = os.path.join(pydrake.getDrakePath(), "examples",
"kuka_iiwa_arm", "models", "objects",
"block_for_pick_and_place.urdf")
AddFlatTerrainToWorld(rbt)
table_frame_robot = RigidBodyFrame("table_frame_robot", rbt.world(),
[0.0, 0, 0], [0, 0, 0])
AddModelInstancesFromSdfString(
open(table_sdf_path).read(), FloatingBaseType.kFixed,
table_frame_robot, rbt)
table_frame_fwd = RigidBodyFrame("table_frame_fwd", rbt.world(),
[0.8, 0, 0], [0, 0, 0])
AddModelInstancesFromSdfString(
open(table_sdf_path).read(), FloatingBaseType.kFixed, table_frame_fwd,
rbt)
table_top_z_in_world = 0.736 + 0.057 / 2
robot_base_frame = RigidBodyFrame("robot_base_frame", rbt.world(),
[0.0, 0, table_top_z_in_world],
[0, 0, 0])
AddModelInstanceFromUrdfFile(iiwa_urdf_path, FloatingBaseType.kFixed,
robot_base_frame, rbt)
object_init_frame = RigidBodyFrame("object_init_frame", rbt.world(),
[0.8, 0, table_top_z_in_world + 0.1],
[0, 0, 0])
AddModelInstanceFromUrdfFile(object_urdf_path,
FloatingBaseType.kRollPitchYaw,
object_init_frame, rbt)
# Add gripper
gripper_frame = rbt.findFrame("iiwa_frame_ee")
AddModelInstancesFromSdfString(
open(wsg50_sdf_path).read(), FloatingBaseType.kFixed, gripper_frame,
rbt)
def do_model_pointcloud_sampling(args, config, vis=None, vis_prefix=None):
# For each class, sample model points on its surface.
for class_i, class_name in enumerate(config["objects"].keys()):
class_rbt = RigidBodyTree()
frame = RigidBodyFrame("frame", class_rbt.world(), np.zeros(3),
np.zeros(3))
model_path = config["objects"][class_name]["model_path"]
_, extension = os.path.splitext(model_path)
if extension == ".urdf":
AddModelInstanceFromUrdfFile(model_path, FloatingBaseType.kFixed,
frame, class_rbt)
elif extension == ".sdf":
AddModelInstancesFromSdfString(
open(model_path).read(), FloatingBaseType.kFixed, frame,
class_rbt)
else:
raise ValueError("Class %s has non-sdf and non-urdf model name." %
class_name)
# Sample model points
model_points, model_normals = sample_points_on_body(
class_rbt, 1, 0.005)
print "Sampled %d model points from model %s" % (model_points.shape[1],
class_name)
save_pointcloud(model_points, model_normals,
os.path.join(args.dir, "%s.pc" % (class_name)))
if vis:
model_pts_offset = (model_points.T + [class_i, 0., -1.0]).T
draw_points(vis,
vis_prefix,
class_name,
model_pts_offset,
model_normals,
size=0.0005,
normals_length=0.00)
def add_single_instance_to_rbt(
rbt,
config,
instance_config,
i,
floating_base_type=FloatingBaseType.kRollPitchYaw):
class_name = instance_config["class"]
if class_name not in config["objects"].keys():
raise ValueError("Class %s not in classes." % class_name)
if len(instance_config["pose"]) != 6:
raise ValueError("Class %s has pose size != 6. Use RPY plz" %
class_name)
frame = RigidBodyFrame("%s_%d" % (class_name, i), rbt.world(),
instance_config["pose"][0:3],
instance_config["pose"][3:6])
model_path = config["objects"][class_name]["model_path"]
_, extension = os.path.splitext(model_path)
if extension == ".urdf":
AddModelInstanceFromUrdfFile(model_path, floating_base_type, frame,
rbt)
elif extension == ".sdf":
AddModelInstancesFromSdfString(
open(model_path).read(), floating_base_type, frame, rbt)
else:
raise ValueError("Class %s has non-sdf and non-urdf model name." %
class_name)
def test_thigh_torque_return_type(self):
"""Verify the signature of ChooseThighTorque"""
from hopper_2d import Hopper2dController
tree = RigidBodyTree()
AddModelInstancesFromSdfString(
open("raibert_hopper_2d.sdf", 'r').read(),
FloatingBaseType.kFixed,
None, tree)
controller = Hopper2dController(tree,
desired_lateral_velocity = 0.0)
x0 = np.zeros(10)
x0[1] = 4. # in air
x0[4] = 0.5 # feasible leg length
torquedes = controller.ChooseThighTorque(x0)
self.assertIsInstance(torquedes, float,
"ChooseThighTorque returned a type other than "\
"float for X0 = %s, desired_lateral_velocity = %f" %
(np.array_str(x0), controller.desired_lateral_velocity))
# Try from another desired velocity
controller.desired_lateral_velocity = -1.0
torquedes = controller.ChooseThighTorque(x0)
self.assertIsInstance(torquedes, float,
"ChooseThighTorque returned a type other than "\
"float for X0 = %s, desired_lateral_velocity = %f" %
(np.array_str(x0), controller.desired_lateral_velocity))
def ConstructVisualizer():
from underactuated import PlanarRigidBodyVisualizer
tree = RigidBodyTree()
AddModelInstancesFromSdfString(
open("raibert_hopper_2d.sdf", 'r').read(), FloatingBaseType.kFixed,
None, tree)
viz = PlanarRigidBodyVisualizer(tree, xlim=[-5, 5], ylim=[-5, 5])
viz.fig.set_size_inches(10, 5)
return viz
def setup_scene(rbt):
AddFlatTerrainToWorld(rbt)
instances = []
num_objects = np.random.randint(10) + 1
for i in range(num_objects):
object_ind = np.random.randint(len(objects.keys()))
pose = np.zeros(6)
pose[0:2] = (np.random.random(2) - 0.5) * 0.05
pose[2] = np.random.random() * 0.1 + 0.05
pose[3:6] = np.random.random(3) * np.pi * 2.
object_init_frame = RigidBodyFrame("object_init_frame", rbt.world(),
pose[0:3], pose[3:6])
object_path = objects[objects.keys()[object_ind]]["model_path"]
if object_path.split(".")[-1] == "urdf":
AddModelInstanceFromUrdfFile(object_path,
FloatingBaseType.kRollPitchYaw,
object_init_frame, rbt)
else:
AddModelInstancesFromSdfString(
open(object_path).read(), FloatingBaseType.kRollPitchYaw,
object_init_frame, rbt)
instances.append({
"class": objects.keys()[object_ind],
"pose": pose.tolist()
})
rbt.compile()
# Project arrangement to nonpenetration with IK
constraints = []
constraints.append(
ik.MinDistanceConstraint(model=rbt,
min_distance=0.01,
active_bodies_idx=list(),
active_group_names=set()))
for body_i in range(2, 1 + num_objects):
constraints.append(
ik.WorldPositionConstraint(model=rbt,
body=body_i,
pts=np.array([0., 0., 0.]),
lb=np.array([-0.5, -0.5, 0.0]),
ub=np.array([0.5, 0.5, 0.5])))
q0 = np.zeros(rbt.get_num_positions())
options = ik.IKoptions(rbt)
options.setDebug(True)
options.setMajorIterationsLimit(10000)
options.setIterationsLimit(100000)
results = ik.InverseKin(rbt, q0, q0, constraints, options)
qf = results.q_sol[0]
info = results.info[0]
print "Projected with info %d" % info
return qf, instances
def Simulate2dBallAndBeam(x0, duration):
builder = DiagramBuilder()
# Load in the ball and beam from a description file.
tree = RigidBodyTree()
AddModelInstancesFromSdfString(
open("ball_and_beam.sdf", 'r').read(), FloatingBaseType.kFixed, None,
tree)
# A RigidBodyPlant wraps a RigidBodyTree to allow
# forward dynamical simulation.
plant = builder.AddSystem(RigidBodyPlant(tree))
# Spawn a controller and hook it up
controller = builder.AddSystem(BallAndBeam2dController(tree))
builder.Connect(plant.get_output_port(0), controller.get_input_port(0))
builder.Connect(controller.get_output_port(0), plant.get_input_port(0))
# Create a logger to log at 30hz
state_log = builder.AddSystem(SignalLogger(plant.get_num_states()))
state_log._DeclarePeriodicPublish(0.0333, 0.0) # 30hz logging
builder.Connect(plant.get_output_port(0), state_log.get_input_port(0))
# Create a simulator
diagram = builder.Build()
simulator = Simulator(diagram)
# Don't limit realtime rate for this sim, since we
# produce a video to render it after simulating the whole thing.
#simulator.set_target_realtime_rate(100.0)
simulator.set_publish_every_time_step(False)
# Force the simulator to use a fixed-step integrator,
# which is much faster for this stiff system. (Due to the
# spring-model of collision, the default variable-timestep
# integrator will take very short steps. I've chosen the step
# size here to be fast while still being stable in most situations.)
integrator = simulator.get_mutable_integrator()
integrator.set_fixed_step_mode(True)
integrator.set_maximum_step_size(0.001)
# Set the initial state
state = simulator.get_mutable_context(
).get_mutable_continuous_state_vector()
state.SetFromVector(x0)
# Simulate!
simulator.StepTo(duration)
return tree, controller, state_log
def Simulate2dHopper(x0,
duration,
desired_lateral_velocity=0.0,
print_period=0.0):
builder = DiagramBuilder()
# Load in the hopper from a description file.
# It's spawned with a fixed floating base because
# the robot description file includes the world as its
# root link -- it does this so that I can create a robot
# system with planar dynamics manually. (Drake doesn't have
# a planar floating base type accessible right now that I
# know about -- it only has 6DOF floating base types.)
tree = RigidBodyTree()
AddModelInstancesFromSdfString(
open("raibert_hopper_2d.sdf", 'r').read(), FloatingBaseType.kFixed,
None, tree)
# A RigidBodyPlant wraps a RigidBodyTree to allow
# forward dynamical simulation. It handles e.g. collision
# modeling.
plant = builder.AddSystem(RigidBodyPlant(tree))
# Alter the ground material used in simulation to make
# it dissipate more energy (to make the hopping more critical)
# and stickier (to make the hopper less likely to slip).
allmaterials = CompliantMaterial()
allmaterials.set_youngs_modulus(1E8) # default 1E9
allmaterials.set_dissipation(1.0) # default 0.32
allmaterials.set_friction(1.0) # default 0.9.
plant.set_default_compliant_material(allmaterials)
# Spawn a controller and hook it up
controller = builder.AddSystem(
Hopper2dController(tree,
desired_lateral_velocity=desired_lateral_velocity,
print_period=print_period))
builder.Connect(plant.get_output_port(0), controller.get_input_port(0))
builder.Connect(controller.get_output_port(0), plant.get_input_port(0))
# Create a logger to log at 30hz
state_log = builder.AddSystem(SignalLogger(plant.get_num_states()))
state_log._DeclarePeriodicPublish(0.0333, 0.0) # 30hz logging
builder.Connect(plant.get_output_port(0), state_log.get_input_port(0))
# Create a simulator
diagram = builder.Build()
simulator = Simulator(diagram)
# Don't limit realtime rate for this sim, since we
# produce a video to render it after simulating the whole thing.
#simulator.set_target_realtime_rate(100.0)
simulator.set_publish_every_time_step(False)
# Force the simulator to use a fixed-step integrator,
# which is much faster for this stiff system. (Due to the
# spring-model of collision, the default variable-timestep
# integrator will take very short steps. I've chosen the step
# size here to be fast while still being stable in most situations.)
integrator = simulator.get_mutable_integrator()
integrator.set_fixed_step_mode(True)
integrator.set_maximum_step_size(0.0005)
# Set the initial state
state = simulator.get_mutable_context(
).get_mutable_continuous_state_vector()
state.SetFromVector(x0)
# Simulate!
simulator.StepTo(duration)
return tree, controller, state_log
