def initialize(load_gazebo=True):
openrave_sim = DynamicSimulationRobotWorld()
robot_xml = XmlSimulationObject("robots/pr2-beta-static.zae",
dynamic=False)
openrave_sim.add_objects([robot_xml], consider_finger_collisions=True)
table = BoxSimulationObject('table', \
[0.66, 0, (TABLE_HEIGHT + 0.03) / 2.0], \
[0.4, 0.75, (TABLE_HEIGHT + 0.03) / 2.0], \
dynamic=False)
openrave_sim.add_objects([table], consider_finger_collisions=True)
#table_xml = TableSimulationObject("table", \
# [0.66, 0, (TABLE_HEIGHT + 0.03) / 2.0],
# [0.4, 0.75, (TABLE_HEIGHT + 0.03) / 2.0],
# dynamic=False)
#openrave_sim.add_objects([table_xml], consider_finger_collisions=True)
cup = XmlSimulationObject(os.path.join(MODELS_DIR, CUP_MESH),
dynamic=False)
openrave_sim.add_objects([cup], consider_finger_collisions=True)
v = trajoptpy.GetViewer(
openrave_sim.env) # TODO: not sure if this is necessary
# Initialize interface for passing controls to Gazebo. Make sure to pass
# in the OpenRave environment we just created, so PR2 isn't referring (and
# updating) a separate OpenRave environment.
if load_gazebo:
pr2 = PR2.PR2(env=openrave_sim.env)
time.sleep(0.2)
else:
pr2 = None
return openrave_sim, pr2, v, cup
def main():
# define simulation objects
table_height = 0.77
cyl_radius = 0.025
cyl_height = 0.3
cyl_pos0 = np.r_[0.7, -0.15, table_height + cyl_height / 2]
cyl_pos1 = np.r_[0.7, 0.15, table_height + cyl_height / 2]
cyl_pos2 = np.r_[0.4, -0.15, table_height + cyl_height / 2]
rope_poss = np.array(
[
[0.2, -0.2, table_height + 0.006],
[0.8, -0.2, table_height + 0.006],
[0.8, 0.2, table_height + 0.006],
[0.2, 0.2, table_height + 0.006],
]
)
sim_objs = []
sim_objs.append(XmlSimulationObject("robots/pr2-beta-static.zae", dynamic=False))
sim_objs.append(BoxSimulationObject("table", [1, 0, table_height - 0.1], [0.85, 0.85, 0.1], dynamic=False))
cyl_sim_objs = create_cylinder_grid(cyl_pos0, cyl_pos1, cyl_pos2, cyl_radius, cyl_height)
sim_objs.extend(cyl_sim_objs)
rope_sim_obj = create_rope(rope_poss)
sim_objs.append(rope_sim_obj)
# initialize simulation world and environment
sim = DynamicSimulationRobotWorld()
sim.add_objects(sim_objs)
sim.create_viewer()
sim.robot.SetDOFValues([0.25], [sim.robot.GetJoint("torso_lift_joint").GetJointIndex()])
sim_util.reset_arms_to_side(sim)
color_cylinders(cyl_sim_objs)
env = environment.LfdEnvironment(sim, sim)
# define augmented trajectory
pick_pos = rope_poss[0] + 0.1 * (rope_poss[1] - rope_poss[0])
drop_pos = rope_poss[3] + 0.1 * (rope_poss[2] - rope_poss[3]) + np.r_[0, 0.2, 0]
pick_R = np.array([[0, 0, 1], [0, 1, 0], [-1, 0, 0]])
drop_R = np.array([[0, 1, 0], [0, 0, -1], [-1, 0, 0]])
move_height = 0.2
aug_traj = create_augmented_traj(sim.robot, pick_pos, drop_pos, pick_R, drop_R, move_height)
env.execute_augmented_trajectory(aug_traj)
def main():
# define simulation objects
table_height = 0.77
sim_objs = []
sim_objs.append(XmlSimulationObject("robots/pr2-beta-static.zae", dynamic=False))
sim_objs.append(BoxSimulationObject("table", [1, 0, table_height-.1], [.85, .85, .1], dynamic=False))
# initialize simulation world and environment
sim = DynamicSimulationRobotWorld()
sim.add_objects(sim_objs)
sim.create_viewer()
sim.robot.SetDOFValues([0.25], [sim.robot.GetJoint('torso_lift_joint').GetJointIndex()])
sim.robot.SetDOFValues([1.25], [sim.robot.GetJoint('head_tilt_joint').GetJointIndex()]) # move head down so it can see the rope
sim_util.reset_arms_to_side(sim)
env = environment.LfdEnvironment(sim, sim, downsample_size=0.025)
demo_rope_poss = np.array([[.2, -.2, table_height+0.006],
[.8, -.2, table_height+0.006],
[.8, .2, table_height+0.006],
[.2, .2, table_height+0.006]])
demo = create_rope_demo(env, demo_rope_poss)
test_rope_poss = np.array([[.2, -.2, table_height+0.006],
[.5, -.4, table_height+0.006],
[.8, .0, table_height+0.006],
[.8, .2, table_height+0.006],
[.6, .0, table_height+0.006],
[.4, .2, table_height+0.006],
[.2, .2, table_height+0.006]])
test_rope_sim_obj = create_rope(test_rope_poss)
sim.add_objects([test_rope_sim_obj])
sim.settle()
test_scene_state = env.observe_scene()
reg_factory = TpsRpmRegistrationFactory()
traj_transferer = FingerTrajectoryTransferer(sim)
plot_cb = lambda i, i_em, x_nd, y_md, xtarg_nd, wt_n, f, corr_nm, rad: registration_plot_cb(sim, x_nd, y_md, f)
reg_and_traj_transferer = TwoStepRegistrationAndTrajectoryTransferer(reg_factory, traj_transferer)
test_aug_traj = reg_and_traj_transferer.transfer(demo, test_scene_state, callback=plot_cb, plotting=True)
env.execute_augmented_trajectory(test_aug_traj)
def main():
# define simulation objects
table_height = 0.77
cyl_radius = 0.025
cyl_height = 0.3
cyl_pos0 = np.r_[.7, -.15, table_height+cyl_height/2]
cyl_pos1 = np.r_[.7, .15, table_height+cyl_height/2]
cyl_pos2 = np.r_[.4, -.15, table_height+cyl_height/2]
rope_poss = np.array([[.2, -.2, table_height+0.006],
[.8, -.2, table_height+0.006],
[.8, .2, table_height+0.006],
[.2, .2, table_height+0.006]])
sim_objs = []
sim_objs.append(XmlSimulationObject("robots/pr2-beta-static.zae", dynamic=False))
sim_objs.append(BoxSimulationObject("table", [1, 0, table_height-.1], [.85, .85, .1], dynamic=False))
cyl_sim_objs = create_cylinder_grid(cyl_pos0, cyl_pos1, cyl_pos2, cyl_radius, cyl_height)
sim_objs.extend(cyl_sim_objs)
rope_sim_obj = create_rope(rope_poss)
sim_objs.append(rope_sim_obj)
# initialize simulation world and environment
sim = DynamicSimulationRobotWorld()
sim.add_objects(sim_objs)
sim.create_viewer()
sim.robot.SetDOFValues([0.25], [sim.robot.GetJoint('torso_lift_joint').GetJointIndex()])
sim_util.reset_arms_to_side(sim)
color_cylinders(cyl_sim_objs)
env = environment.LfdEnvironment(sim, sim)
# define augmented trajectory
pick_pos = rope_poss[0] + .1 * (rope_poss[1] - rope_poss[0])
drop_pos = rope_poss[3] + .1 * (rope_poss[2] - rope_poss[3]) + np.r_[0, .2, 0]
pick_R = np.array([[0, 0, 1], [0, 1, 0], [-1, 0, 0]])
drop_R = np.array([[0, 1, 0], [0, 0, -1], [-1, 0, 0]])
move_height = .2
aug_traj = create_augmented_traj(sim.robot, pick_pos, drop_pos, pick_R, drop_R, move_height)
env.execute_augmented_trajectory(aug_traj)
init_rope_nodes = np.r_[mu.linspace2d(rope_pos0, rope_pos1, np.round(np.linalg.norm(rope_pos1 - rope_pos0)/.02)),
mu.linspace2d(rope_pos1, rope_pos2, np.round(np.linalg.norm(rope_pos2 - rope_pos1)/.02))[1:,:],
mu.linspace2d(rope_pos2, rope_pos3, np.round(np.linalg.norm(rope_pos3 - rope_pos2)/.02))[1:,:]]
rope_params = sim_util.RopeParams()
sim_objs = []
sim_objs.append(XmlSimulationObject("robots/pr2-beta-static.zae", dynamic=False))
sim_objs.append(BoxSimulationObject("table", [1, 0, table_height-.1], [.85, .85, .1], dynamic=False))
# add grid of cylinders
cyl_sim_objs = []
for (i,cyl_pos) in enumerate(cyl_positions):
cyl_sim_objs.append(CylinderSimulationObject("cyl%i"%i, cyl_pos, cyl_radius, cyl_height, dynamic=True))
sim_objs.extend(cyl_sim_objs)
sim_objs.append(RopeSimulationObject("rope", init_rope_nodes, rope_params))
sim = DynamicSimulationRobotWorld()
sim.add_objects(sim_objs)
sim.robot.SetDOFValues([0.25], [sim.robot.GetJoint('torso_lift_joint').GetJointIndex()])
sim_util.reset_arms_to_side(sim)
# set random colors to cylinders
for sim_obj in cyl_sim_objs:
color = np.random.random(3)
for bt_obj in sim_obj.get_bullet_objects():
for link in bt_obj.GetKinBody().GetLinks():
for geom in link.GetGeometries():
geom.SetDiffuseColor(color)
viewer = trajoptpy.GetViewer(sim.env)
camera_matrix = np.array([[ 0, 1, 0, 0],
[-1, 0, 0.5, 0],
from lfd.environment.simulation_object import XmlSimulationObject, BoxSimulationObject
from lfd.environment import sim_util
table_height = 0.77
box_length = 0.04
box_depth = 0.12
box0_pos = np.r_[.6, -.3, table_height+box_depth/2]
box1_pos = np.r_[.6, 0, table_height+box_depth/2]
sim_objs = []
sim_objs.append(XmlSimulationObject("robots/pr2-beta-static.zae", dynamic=False))
sim_objs.append(BoxSimulationObject("table", [1, 0, table_height-.1], [.85, .85, .1], dynamic=False))
sim_objs.append(BoxSimulationObject("box0", box0_pos, [box_length/2, box_length/2, box_depth/2], dynamic=True))
sim_objs.append(BoxSimulationObject("box1", box1_pos, [box_length/2, box_length/2, box_depth/2], dynamic=True))
sim = DynamicSimulationRobotWorld()
sim.add_objects(sim_objs)
sim.create_viewer()
sim.robot.SetDOFValues([0.25], [sim.robot.GetJoint('torso_lift_joint').GetJointIndex()])
sim_util.reset_arms_to_side(sim)
# rotate box0 by 22.5 degrees
bt_box0 = sim.bt_env.GetObjectByName('box0')
T = openravepy.matrixFromAxisAngle(np.array([0,0,np.pi/8]))
T[:3,3] = bt_box0.GetTransform()[:3,3]
bt_box0.SetTransform(T) # SetTransform needs to be used in the Bullet object, not the openrave body
sim.update()
lr = 'r'
manip_name = {"l":"leftarm", "r":"rightarm"}[lr]
XmlSimulationObject("robots/pr2-beta-static.zae", dynamic=False))
sim_objs.append(
BoxSimulationObject("table", [1, 0, table_height - .1], [.85, .85, .1],
dynamic=False))
sim_objs.append(
BoxSimulationObject("box0",
box0_pos,
[box_length / 2, box_length / 2, box_depth / 2],
dynamic=True))
sim_objs.append(
BoxSimulationObject("box1",
box1_pos,
[box_length / 2, box_length / 2, box_depth / 2],
dynamic=True))
sim = DynamicSimulationRobotWorld()
sim.add_objects(sim_objs)
sim.create_viewer()
sim.robot.SetDOFValues(
[0.25], [sim.robot.GetJoint('torso_lift_joint').GetJointIndex()])
sim_util.reset_arms_to_side(sim)
# rotate box0 by 22.5 degrees
bt_box0 = sim.bt_env.GetObjectByName('box0')
T = openravepy.matrixFromAxisAngle(np.array([0, 0, np.pi / 8]))
T[:3, 3] = bt_box0.GetTransform()[:3, 3]
bt_box0.SetTransform(
T
) # SetTransform needs to be used in the Bullet object, not the openrave body
sim.update()
