def setup_initial_world(self, n_objects):
# Construct the initial robot and its environment
rbt = RigidBodyTree()
self.setup_kuka(rbt)
rbt_just_kuka = rbt.Clone()
rbt_just_kuka.compile()
# Figure out initial pose for the arm
ee_body = rbt_just_kuka.FindBody("right_finger").get_body_index()
ee_point = np.array([0.0, 0.03, 0.0])
end_effector_desired = np.array(
[0.5, 0.0, self.table_top_z_in_world + 0.5, -np.pi / 2., 0., 0.])
q0_kuka_seed = rbt_just_kuka.getZeroConfiguration()
# "Center low" from IIWA stored_poses.json from Spartan
# + closed hand + raised blade
q0_kuka_seed[0:9] = np.array(
[-0.18, -1., 0.12, -1.89, 0.1, 1.3, 0.38, 0.0, 0.0])
if self.with_knife:
q0_kuka_seed[9] = 1.5
q0_kuka, info = kuka_ik.plan_ee_configuration(rbt_just_kuka,
q0_kuka_seed,
q0_kuka_seed,
end_effector_desired,
ee_body,
ee_point,
allow_collision=True,
euler_limits=0.01)
if info != 1:
print "Info %d on IK for initial posture." % info
# Add objects + make random initial poses
q0 = np.zeros(rbt.get_num_positions() + 6 * n_objects)
q0[0:rbt_just_kuka.get_num_positions()] = q0_kuka
for k in range(n_objects):
self.add_cut_cylinder_to_tabletop(rbt, cut_dirs=[], cut_points=[])
radius = self.manipuland_params[-1]["radius"]
new_body = rbt.get_body(self.manipuland_body_indices[-1])
# Remember to reverse effects of self.magic_rpy_offset
new_pos = self.magic_rpy_rotmat.T.dot(
np.array([
0.4 + np.random.random() * 0.2,
-0.2 + np.random.random() * 0.4,
self.table_top_z_in_world + radius + 0.001
]))
new_rot = (np.random.random(3) * np.pi * 2.) - \
self.magic_rpy_offset
q0[range(new_body.get_position_start_index(),
new_body.get_position_start_index() + 6)] = np.hstack(
[new_pos, new_rot])
rbt.compile()
q0_feas = self.project_rbt_to_nearest_feasible_on_table(rbt, q0)
return rbt, rbt_just_kuka, q0_feas
def test_box_container():
# Empty rigidbody tree
rbt = RigidBodyTree()
# The iiwa link path
add_robot(rbt)
# The box
container_config = BoxContainerConfig()
container_config.dim_xyz = [2, 1, 1]
container_config.center_xyz = [0, 0, 0]
add_box_container_to_rbt(rbt, container_config)
rbt.compile()
# Visualize the box
from kplan.visualizer.meshcat_wrapper import MeshCatVisualizer
visualizer = MeshCatVisualizer()
visualizer.add_rigidbody_tree(rbt,
rbt.getZeroConfiguration(),
draw_collision=False)
def test_box_shape():
# Empty rigidbody tree
rbt = RigidBodyTree()
# The iiwa link path
add_robot(rbt)
# The box
box_config = BoxConfig()
box_config.dim_xyz = [1, 1, 1]
box_config.box_in_world = np.eye(4)
add_box_to_rbt(rbt, box_config)
rbt.compile()
# Visualize the box
from kplan.visualizer.meshcat_wrapper import MeshCatVisualizer
visualizer = MeshCatVisualizer()
visualizer.add_rigidbody_tree(rbt,
rbt.getZeroConfiguration(),
draw_collision=True)
def test_cylinder():
# Empty rigidbody tree
rbt = RigidBodyTree()
# The iiwa link path
add_robot(rbt)
# Compute the transform
import kplan.utils.transformations as transformations
quat = [0.6784430069620968, 0, 0, 0.7346530380419237]
rack2world = transformations.quaternion_matrix(quat)
rack2world[0, 3] = 0.3285152706168804
rack2world[1, 3] = 0.007816573364084667
rack2world[2, 3] = -0.045688056593718634
start_point = np.array([0.64248, -0.00062, 0.2854])
end_point = [0.56776, 0.00287, 0.31459]
transformed_start_point = rack2world[0:3, 0:3].dot(
start_point) + rack2world[0:3, 3]
transformed_end_point = rack2world[0:3,
0:3].dot(end_point) + rack2world[0:3, 3]
# The cylinder
cylinder = CylinderConfig()
cylinder.start_point = []
cylinder.end_point = []
for i in range(3):
cylinder.start_point.append(float(transformed_start_point[i]))
cylinder.end_point.append(float(transformed_end_point[i]))
cylinder.radius = 0.02
print(cylinder.start_point)
print(cylinder.end_point)
add_cylinder_to_rbt(rbt, cylinder)
rbt.compile()
# Visualize the cylinder
from kplan.visualizer.meshcat_wrapper import MeshCatVisualizer
visualizer = MeshCatVisualizer()
visualizer.add_rigidbody_tree(rbt,
rbt.getZeroConfiguration(),
draw_collision=False)
width=width,
height=height,
show_window=False)
camera.set_color_camera_optical_pose(camera.depth_camera_optical_pose())
depth_camera_pose = camera.depth_camera_optical_pose().matrix()
# Build RBTs with each object individually present in them for
# doing distance checks, and for generating model point clouds
q0 = np.zeros(6)
single_object_rbts = []
for instance_j, instance_config in enumerate(config["instances"]):
new_rbt = RigidBodyTree()
add_single_instance_to_rbt(new_rbt, config, instance_config,
instance_j)
new_rbt.compile()
single_object_rbts.append(new_rbt)
all_points = []
all_points_normals = []
all_points_labels = []
all_points_distances = [[] for i in range(len(config["instances"]))]
for i, viewpoint in enumerate(camera_config["viewpoints"]):
camera_tf = lookat(viewpoint["eye"], viewpoint["target"],
viewpoint["up"])
camera_tf = camera_tf.dot(transform_inverse(depth_camera_pose))
camera_rpy = RollPitchYaw(RotationMatrix(camera_tf[0:3, 0:3]))
q0 = np.zeros(6)
q0[0:3] = camera_tf[0:3, 3]
q0[3:6] = camera_rpy.vector()
from pydrake.examples.compass_gait import (CompassGait, CompassGaitParams)
from underactuated import (PlanarRigidBodyVisualizer)
tree = RigidBodyTree(
FindResourceOrThrow("drake/examples/compass_gait/CompassGait.urdf"),
FloatingBaseType.kRollPitchYaw)
params = CompassGaitParams()
R = np.identity(3)
R[0, 0] = math.cos(params.slope())
R[0, 2] = math.sin(params.slope())
R[2, 0] = -math.sin(params.slope())
R[2, 2] = math.cos(params.slope())
X = Isometry3(rotation=R, translation=[0, 0, -5.])
color = np.array([0.9297, 0.7930, 0.6758, 1])
tree.world().AddVisualElement(VisualElement(Box([100., 1., 10.]), X, color))
tree.compile()
builder = DiagramBuilder()
compass_gait = builder.AddSystem(CompassGait())
parser = argparse.ArgumentParser()
parser.add_argument("-T",
"--duration",
type=float,
help="Duration to run sim.",
default=10.0)
args = parser.parse_args()
visualizer = builder.AddSystem(
PlanarRigidBodyVisualizer(tree,
xlim=[-1., 5.],
def do_cut(self, rbt, x, cut_body_index, cut_pt, cut_normal):
# Rebuilds the full rigid body tree, replacing cut_body_index
# with one that is cut, but otherwise keeping the rest of the
# tree the same. The new tree won't have the same body indices
# (for the manipulands and anything added after them) as the
# original.
old_manipuland_indices = deepcopy(self.manipuland_body_indices)
old_manipuland_params = deepcopy(self.manipuland_params)
self.__init__()
new_rbt = RigidBodyTree()
self.setup_kuka(new_rbt)
q_new = np.zeros(rbt.get_num_positions() + 6)
v_new = np.zeros(rbt.get_num_positions() + 6)
if rbt.get_num_positions() != rbt.get_num_velocities():
raise Exception("Can't handle nq != nv, sorry...")
def copy_state(from_indices, to_indices):
q_new[to_indices] = x[from_indices]
v_new[to_indices] = x[np.array(from_indices) +
rbt.get_num_positions()]
copy_state(range(new_rbt.get_num_positions()),
range(new_rbt.get_num_positions()))
k = 0
for i, ind in enumerate(old_manipuland_indices):
print i, ind
p = old_manipuland_params[i]
if ind is cut_body_index:
for sign in [-1., 1.]:
try:
self.add_cut_cylinder_to_tabletop(
new_rbt,
height=p["height"],
radius=p["radius"],
cut_dirs=p["cut_dirs"] + [cut_normal * sign],
cut_points=p["cut_points"] +
[cut_pt + cut_normal * sign * 0.002])
except subprocess.CalledProcessError as e:
print "Failed a cut: ", e
continue # failed to cut
k += 1
copy_state(
range(
rbt.get_body(ind).get_position_start_index(),
rbt.get_body(ind).get_position_start_index() + 6),
range(new_rbt.get_num_positions() - 6,
new_rbt.get_num_positions()))
else:
self.add_cut_cylinder_to_tabletop(new_rbt,
height=p["height"],
radius=p["radius"],
cut_dirs=p["cut_dirs"],
cut_points=p["cut_points"])
copy_state(
range(
rbt.get_body(ind).get_position_start_index(),
rbt.get_body(ind).get_position_start_index() + 6),
range(new_rbt.get_num_positions() - 6,
new_rbt.get_num_positions()))
k += 1
# Account for possible cut failures
q_new = q_new[:new_rbt.get_num_positions()]
v_new = v_new[:new_rbt.get_num_velocities()]
# Map old state into new state
new_rbt.compile()
q_new = self.project_rbt_to_nearest_feasible_on_table(new_rbt, q_new)
return new_rbt, np.hstack([q_new, v_new])
from underactuated import (PlanarRigidBodyVisualizer)
tree = RigidBodyTree(FindResourceOrThrow(
"drake/examples/compass_gait/CompassGait.urdf"),
FloatingBaseType.kRollPitchYaw)
params = CompassGaitParams()
R = np.identity(3)
R[0, 0] = math.cos(params.slope())
R[0, 2] = math.sin(params.slope())
R[2, 0] = -math.sin(params.slope())
R[2, 2] = math.cos(params.slope())
X = Isometry3(rotation=R, translation=[0, 0, -5.])
color = np.array([0.9297, 0.7930, 0.6758, 1])
tree.world().AddVisualElement(VisualElement(Box([100., 1., 10.]), X, color))
tree.compile()
builder = DiagramBuilder()
compass_gait = builder.AddSystem(CompassGait())
hip_torque = builder.AddSystem(ConstantVectorSource([0.0]))
builder.Connect(hip_torque.get_output_port(0), compass_gait.get_input_port(0))
parser = argparse.ArgumentParser()
parser.add_argument("-T", "--duration",
type=float,
help="Duration to run sim.",
default=10.0)
args = parser.parse_args()
visualizer = builder.AddSystem(PlanarRigidBodyVisualizer(tree,