Esempio n. 1
0
def setup_lfd_environment_sim(args):
    actions = h5py.File(args.gen_tasks.actionfile, 'r')

    init_rope_xyz, init_joint_names, init_joint_values = sim_util.load_fake_data_segment(
        actions, args.gen_tasks.fake_data_segment,
        args.gen_tasks.fake_data_transform)
    table_height = init_rope_xyz[:, 2].mean() - .02
    sim_objs = []
    sim_objs.append(
        BoxSimulationObject("table", [1, 0, table_height + (-.1 + .01)],
                            [.85, .85, .1],
                            dynamic=False))

    sim = DynamicSimulation()
    world = sim
    sim.add_objects(sim_objs)

    if args.animation:
        viewer = trajoptpy.GetViewer(sim.env)
        if os.path.isfile(args.window_prop_file) and os.path.isfile(
                args.camera_matrix_file):
            print "loading window and camera properties"
            window_prop = np.loadtxt(args.window_prop_file)
            camera_matrix = np.loadtxt(args.camera_matrix_file)
            try:
                viewer.SetWindowProp(*window_prop)
                viewer.SetCameraManipulatorMatrix(camera_matrix)
            except:
                print "SetWindowProp and SetCameraManipulatorMatrix are not defined. Pull and recompile Trajopt."
        else:
            print "move viewer to viewpoint that isn't stupid"
            print "then hit 'p' to continue"
            viewer.Idle()
            print "saving window and camera properties"
            try:
                window_prop = viewer.GetWindowProp()
                camera_matrix = viewer.GetCameraManipulatorMatrix()
                np.savetxt(args.window_prop_file, window_prop, fmt='%d')
                np.savetxt(args.camera_matrix_file, camera_matrix)
            except:
                print "GetWindowProp and GetCameraManipulatorMatrix are not defined. Pull and recompile Trajopt."
        viewer.Step()

    return sim
Esempio n. 2
0
def setup_lfd_environment_sim(args):
    actions = h5py.File(args.gen_tasks.actionfile, 'r')

    init_rope_xyz, init_joint_names, init_joint_values = sim_util.load_fake_data_segment(actions, args.gen_tasks.fake_data_segment, args.gen_tasks.fake_data_transform)
    table_height = init_rope_xyz[:,2].mean() - .02
    sim_objs = []
    sim_objs.append(BoxSimulationObject("table",
                                        [1, 0, table_height + (-.1 + .01)],
                                        [.85, .85, .1], dynamic=False))

    sim = DynamicSimulation()
    world = sim
    sim.add_objects(sim_objs)

    if args.animation:
        viewer = trajoptpy.GetViewer(sim.env)
        if os.path.isfile(args.window_prop_file) and os.path.isfile(args.camera_matrix_file):
            print "loading window and camera properties"
            window_prop = np.loadtxt(args.window_prop_file)
            camera_matrix = np.loadtxt(args.camera_matrix_file)
            try:
                viewer.SetWindowProp(*window_prop)
                viewer.SetCameraManipulatorMatrix(camera_matrix)
            except:
                print "SetWindowProp and SetCameraManipulatorMatrix are not defined. Pull and recompile Trajopt."
        else:
            print "move viewer to viewpoint that isn't stupid"
            print "then hit 'p' to continue"
            viewer.Idle()
            print "saving window and camera properties"
            try:
                window_prop = viewer.GetWindowProp()
                camera_matrix = viewer.GetCameraManipulatorMatrix()
                np.savetxt(args.window_prop_file, window_prop, fmt='%d')
                np.savetxt(args.camera_matrix_file, camera_matrix)
            except:
                print "GetWindowProp and GetCameraManipulatorMatrix are not defined. Pull and recompile Trajopt."
        viewer.Step()

    return sim
Esempio n. 3
0
    def setUp(self):
        table_height = 0.77
        helix_ang0 = 0
        helix_ang1 = 4*np.pi
        helix_radius = .2
        helix_center = np.r_[.6, 0]
        helix_height0 = table_height + .15
        helix_height1 = table_height + .15 + .3
        helix_length = np.linalg.norm(np.r_[(helix_ang1 - helix_ang0) * helix_radius, helix_height1 - helix_height0])
        num = np.round(helix_length/.02)
        helix_angs = np.linspace(helix_ang0, helix_ang1, num)
        helix_heights = np.linspace(helix_height0, helix_height1, num)
        init_rope_nodes = np.c_[helix_center + helix_radius * np.c_[np.cos(helix_angs), np.sin(helix_angs)], helix_heights]
        rope_params = sim_util.RopeParams()
        
        cyl_radius = 0.025
        cyl_height = 0.3
        cyl_pos0 = np.r_[.6, helix_radius, table_height + .25]
        cyl_pos1 = np.r_[.6, -helix_radius, table_height + .35]
        
        sim_objs = []
        sim_objs.append(BoxSimulationObject("table", [1, 0, table_height-.1], [.85, .85, .1], dynamic=False))
        sim_objs.append(RopeSimulationObject("rope", init_rope_nodes, rope_params))
        sim_objs.append(CylinderSimulationObject("cyl0", cyl_pos0, cyl_radius, cyl_height, dynamic=True))
        sim_objs.append(CylinderSimulationObject("cyl1", cyl_pos1, cyl_radius, cyl_height, dynamic=True))
        
        self.sim = DynamicSimulation()
        self.sim.add_objects(sim_objs)

        # rotate cylinders by 90 deg
        for i in range(2):
            bt_cyl = self.sim.bt_env.GetObjectByName('cyl%d' % i)
            T = openravepy.matrixFromAxisAngle(np.array([np.pi/2, 0, 0]))
            T[:3,3] = bt_cyl.GetTransform()[:3, 3]
            bt_cyl.SetTransform(T)  # SetTransform needs to be used in the Bullet object, not the openrave body
        self.sim.update()

        self.sim.add_objects([XmlSimulationObject("robots/pr2-beta-static.zae", dynamic=False)])
        self.sim.robot.SetDOFValues([0.25], [self.sim.robot.GetJoint('torso_lift_joint').GetJointIndex()])
        sim_util.reset_arms_to_side(self.sim)
Esempio n. 4
0
init_rope_nodes = np.c_[helix_center + helix_radius * np.c_[np.cos(helix_angs), np.sin(helix_angs)], helix_heights]
rope_params = sim_util.RopeParams()

cyl_radius = 0.025
cyl_height = 0.3
cyl_pos0 = np.r_[.6, helix_radius, table_height + .25]
cyl_pos1 = np.r_[.6, -helix_radius, table_height + .35]

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(RopeSimulationObject("rope", init_rope_nodes, rope_params))
sim_objs.append(CylinderSimulationObject("cyl0", cyl_pos0, cyl_radius, cyl_height, dynamic=True))
sim_objs.append(CylinderSimulationObject("cyl1", cyl_pos1, cyl_radius, cyl_height, dynamic=True))

sim = DynamicSimulation()
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 cylinders by 90 deg
for i in range(2):
    bt_cyl = sim.bt_env.GetObjectByName('cyl%d'%i)
    T = openravepy.matrixFromAxisAngle(np.array([np.pi/2,0,0]))
    T[:3,3] = bt_cyl.GetTransform()[:3,3]
    bt_cyl.SetTransform(T) # SetTransform needs to be used in the Bullet object, not the openrave body
sim.update()

sim.settle(max_steps=1000)
Esempio n. 5
0
init_rope_nodes = np.c_[helix_center + helix_radius * np.c_[np.cos(helix_angs), np.sin(helix_angs)], helix_heights]
rope_params = sim_util.RopeParams()

cyl_radius = 0.025
cyl_height = 0.3
cyl_pos0 = np.r_[.6, helix_radius, table_height + .25]
cyl_pos1 = np.r_[.6, -helix_radius, table_height + .35]

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(RopeSimulationObject("rope", init_rope_nodes, rope_params))
sim_objs.append(CylinderSimulationObject("cyl0", cyl_pos0, cyl_radius, cyl_height, dynamic=True))
sim_objs.append(CylinderSimulationObject("cyl1", cyl_pos1, cyl_radius, cyl_height, dynamic=True))

sim = DynamicSimulation()
sim.add_objects(sim_objs)
sim.robot.SetDOFValues([0.25], [sim.robot.GetJoint('torso_lift_joint').GetJointIndex()])
sim_util.reset_arms_to_side(sim)

viewer = trajoptpy.GetViewer(sim.env)
camera_matrix = np.array([[ 0,    1, 0,   0],
                          [-1,    0, 0.5, 0],
                          [ 0.5,  0, 1,   0],
                          [ 2.25, 0, 4.5, 1]])
viewer.SetWindowProp(0,0,1500,1500)
viewer.SetCameraManipulatorMatrix(camera_matrix)

# rotate cylinders by 90 deg
for i in range(2):
    bt_cyl = sim.bt_env.GetObjectByName('cyl%d'%i)
Esempio n. 6
0
class TestSimulation(unittest.TestCase):
    def setUp(self):
        table_height = 0.77
        helix_ang0 = 0
        helix_ang1 = 4*np.pi
        helix_radius = .2
        helix_center = np.r_[.6, 0]
        helix_height0 = table_height + .15
        helix_height1 = table_height + .15 + .3
        helix_length = np.linalg.norm(np.r_[(helix_ang1 - helix_ang0) * helix_radius, helix_height1 - helix_height0])
        num = np.round(helix_length/.02)
        helix_angs = np.linspace(helix_ang0, helix_ang1, num)
        helix_heights = np.linspace(helix_height0, helix_height1, num)
        init_rope_nodes = np.c_[helix_center + helix_radius * np.c_[np.cos(helix_angs), np.sin(helix_angs)], helix_heights]
        rope_params = sim_util.RopeParams()
        
        cyl_radius = 0.025
        cyl_height = 0.3
        cyl_pos0 = np.r_[.6, helix_radius, table_height + .25]
        cyl_pos1 = np.r_[.6, -helix_radius, table_height + .35]
        
        sim_objs = []
        sim_objs.append(BoxSimulationObject("table", [1, 0, table_height-.1], [.85, .85, .1], dynamic=False))
        sim_objs.append(RopeSimulationObject("rope", init_rope_nodes, rope_params))
        sim_objs.append(CylinderSimulationObject("cyl0", cyl_pos0, cyl_radius, cyl_height, dynamic=True))
        sim_objs.append(CylinderSimulationObject("cyl1", cyl_pos1, cyl_radius, cyl_height, dynamic=True))
        
        self.sim = DynamicSimulation()
        self.sim.add_objects(sim_objs)

        # rotate cylinders by 90 deg
        for i in range(2):
            bt_cyl = self.sim.bt_env.GetObjectByName('cyl%d' % i)
            T = openravepy.matrixFromAxisAngle(np.array([np.pi/2, 0, 0]))
            T[:3,3] = bt_cyl.GetTransform()[:3, 3]
            bt_cyl.SetTransform(T)  # SetTransform needs to be used in the Bullet object, not the openrave body
        self.sim.update()

        self.sim.add_objects([XmlSimulationObject("robots/pr2-beta-static.zae", dynamic=False)])
        self.sim.robot.SetDOFValues([0.25], [self.sim.robot.GetJoint('torso_lift_joint').GetJointIndex()])
        sim_util.reset_arms_to_side(self.sim)
    
    def test_reproducibility(self):
        sim_state0 = self.sim.get_state()
        
        self.sim.set_state(sim_state0)
        self.sim.settle(max_steps=1000)
        sim_state1 = self.sim.get_state()
        
        self.sim.set_state(sim_state0)
        self.sim.settle(max_steps=1000)
        sim_state2 = self.sim.get_state()
        
        self.assertArrayDictEqual(sim_state1[1], sim_state2[1])
    
    def test_viewer_side_effects(self):
        """
        Check if stepping the viewer has side effects in the simulation
        """
        sim_state0 = self.sim.get_state()
        
        self.sim.set_state(sim_state0)
        self.sim.settle(max_steps=1000, step_viewer=0)
        sim_state1 = self.sim.get_state()
        
        # create viewer
        viewer = trajoptpy.GetViewer(self.sim.env)
        
        self.sim.set_state(sim_state0)
        self.sim.settle(max_steps=1000, step_viewer=10)
        sim_state2 = self.sim.get_state()
        
        self.assertArrayDictEqual(sim_state1[1], sim_state2[1])
    
    def test_remove_sim_obj(self):
        sim_state0 = self.sim.get_state()
        
        self.sim.set_state(sim_state0)
        self.sim.settle(max_steps=1000)
        sim_state1 = self.sim.get_state()
        
        rope = [sim_obj for sim_obj in self.sim.sim_objs if isinstance(sim_obj, RopeSimulationObject)][0]
        box = BoxSimulationObject("box", [0]*3, [.1]*3, dynamic=False)

        self.sim.remove_objects([rope])
        self.sim.set_state(sim_state0)
        self.sim.settle(max_steps=1000)
        sim_state2 = self.sim.get_state() # this adds another rope that has the same properties as rope
        
        self.sim.add_objects([box])
        self.sim.set_state(sim_state0)
        self.sim.settle(max_steps=1000)
        sim_state3 = self.sim.get_state() # this removes the recently added box
        
        rope = [sim_obj for sim_obj in self.sim.sim_objs if isinstance(sim_obj, RopeSimulationObject)][0]
        
        self.sim.remove_objects([rope])
        self.sim.add_objects([box])
        self.sim.set_state(sim_state0)
        self.sim.settle(max_steps=1000)
        sim_state4 = self.sim.get_state()
        
        self.assertArrayDictEqual(sim_state1[1], sim_state2[1])
        self.assertArrayDictEqual(sim_state1[1], sim_state3[1])
        self.assertArrayDictEqual(sim_state1[1], sim_state4[1])
    
    def assertArrayDictEqual(self, d0, d1):
        self.assertSetEqual(set(d0.keys()), set(d1.keys()))
        for (k, v0) in d0.iteritems():
            v1 = d1[k]
            self.assertTrue(np.all(v0 == v1))
Esempio n. 7
0
                        dynamic=False))
sim_objs.append(RopeSimulationObject("rope", init_rope_nodes, rope_params))
sim_objs.append(
    CylinderSimulationObject("cyl0",
                             cyl_pos0,
                             cyl_radius,
                             cyl_height,
                             dynamic=True))
sim_objs.append(
    CylinderSimulationObject("cyl1",
                             cyl_pos1,
                             cyl_radius,
                             cyl_height,
                             dynamic=True))

sim = DynamicSimulation()
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 cylinders by 90 deg
for i in range(2):
    bt_cyl = sim.bt_env.GetObjectByName('cyl%d' % i)
    T = openravepy.matrixFromAxisAngle(np.array([np.pi / 2, 0, 0]))
    T[:3, 3] = bt_cyl.GetTransform()[:3, 3]
    bt_cyl.SetTransform(
        T
    )  # SetTransform needs to be used in the Bullet object, not the openrave body
Esempio n. 8
0
    def setUp(self):
        table_height = 0.77
        helix_ang0 = 0
        helix_ang1 = 4 * np.pi
        helix_radius = .2
        helix_center = np.r_[.6, 0]
        helix_height0 = table_height + .15
        helix_height1 = table_height + .15 + .3
        helix_length = np.linalg.norm(
            np.r_[(helix_ang1 - helix_ang0) * helix_radius,
                  helix_height1 - helix_height0])
        num = np.round(helix_length / .02)
        helix_angs = np.linspace(helix_ang0, helix_ang1, num)
        helix_heights = np.linspace(helix_height0, helix_height1, num)
        init_rope_nodes = np.c_[helix_center +
                                helix_radius * np.c_[np.cos(helix_angs),
                                                     np.sin(helix_angs)],
                                helix_heights]
        rope_params = sim_util.RopeParams()

        cyl_radius = 0.025
        cyl_height = 0.3
        cyl_pos0 = np.r_[.6, helix_radius, table_height + .25]
        cyl_pos1 = np.r_[.6, -helix_radius, table_height + .35]

        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(
            RopeSimulationObject("rope", init_rope_nodes, rope_params))
        sim_objs.append(
            CylinderSimulationObject("cyl0",
                                     cyl_pos0,
                                     cyl_radius,
                                     cyl_height,
                                     dynamic=True))
        sim_objs.append(
            CylinderSimulationObject("cyl1",
                                     cyl_pos1,
                                     cyl_radius,
                                     cyl_height,
                                     dynamic=True))

        self.sim = DynamicSimulation()
        self.sim.add_objects(sim_objs)
        self.sim.robot.SetDOFValues(
            [0.25],
            [self.sim.robot.GetJoint('torso_lift_joint').GetJointIndex()])
        sim_util.reset_arms_to_side(self.sim)

        # rotate cylinders by 90 deg
        for i in range(2):
            bt_cyl = self.sim.bt_env.GetObjectByName('cyl%d' % i)
            T = openravepy.matrixFromAxisAngle(np.array([np.pi / 2, 0, 0]))
            T[:3, 3] = bt_cyl.GetTransform()[:3, 3]
            bt_cyl.SetTransform(
                T
            )  # SetTransform needs to be used in the Bullet object, not the openrave body
        self.sim.update()
Esempio n. 9
0
class TestSimulation(unittest.TestCase):
    def setUp(self):
        table_height = 0.77
        helix_ang0 = 0
        helix_ang1 = 4 * np.pi
        helix_radius = .2
        helix_center = np.r_[.6, 0]
        helix_height0 = table_height + .15
        helix_height1 = table_height + .15 + .3
        helix_length = np.linalg.norm(
            np.r_[(helix_ang1 - helix_ang0) * helix_radius,
                  helix_height1 - helix_height0])
        num = np.round(helix_length / .02)
        helix_angs = np.linspace(helix_ang0, helix_ang1, num)
        helix_heights = np.linspace(helix_height0, helix_height1, num)
        init_rope_nodes = np.c_[helix_center +
                                helix_radius * np.c_[np.cos(helix_angs),
                                                     np.sin(helix_angs)],
                                helix_heights]
        rope_params = sim_util.RopeParams()

        cyl_radius = 0.025
        cyl_height = 0.3
        cyl_pos0 = np.r_[.6, helix_radius, table_height + .25]
        cyl_pos1 = np.r_[.6, -helix_radius, table_height + .35]

        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(
            RopeSimulationObject("rope", init_rope_nodes, rope_params))
        sim_objs.append(
            CylinderSimulationObject("cyl0",
                                     cyl_pos0,
                                     cyl_radius,
                                     cyl_height,
                                     dynamic=True))
        sim_objs.append(
            CylinderSimulationObject("cyl1",
                                     cyl_pos1,
                                     cyl_radius,
                                     cyl_height,
                                     dynamic=True))

        self.sim = DynamicSimulation()
        self.sim.add_objects(sim_objs)
        self.sim.robot.SetDOFValues(
            [0.25],
            [self.sim.robot.GetJoint('torso_lift_joint').GetJointIndex()])
        sim_util.reset_arms_to_side(self.sim)

        # rotate cylinders by 90 deg
        for i in range(2):
            bt_cyl = self.sim.bt_env.GetObjectByName('cyl%d' % i)
            T = openravepy.matrixFromAxisAngle(np.array([np.pi / 2, 0, 0]))
            T[:3, 3] = bt_cyl.GetTransform()[:3, 3]
            bt_cyl.SetTransform(
                T
            )  # SetTransform needs to be used in the Bullet object, not the openrave body
        self.sim.update()

    def test_reproducibility(self):
        sim_state0 = self.sim.get_state()

        self.sim.set_state(sim_state0)
        self.sim.settle(max_steps=1000)
        sim_state1 = self.sim.get_state()

        self.sim.set_state(sim_state0)
        self.sim.settle(max_steps=1000)
        sim_state2 = self.sim.get_state()

        self.assertArrayDictEqual(sim_state1[1], sim_state2[1])

    def test_viewer_side_effects(self):
        """
        Check if stepping the viewer has side effects in the simulation
        """
        sim_state0 = self.sim.get_state()

        self.sim.set_state(sim_state0)
        self.sim.settle(max_steps=1000, step_viewer=0)
        sim_state1 = self.sim.get_state()

        # create viewer
        viewer = trajoptpy.GetViewer(self.sim.env)

        self.sim.set_state(sim_state0)
        self.sim.settle(max_steps=1000, step_viewer=10)
        sim_state2 = self.sim.get_state()

        self.assertArrayDictEqual(sim_state1[1], sim_state2[1])

    def test_remove_sim_obj(self):
        sim_state0 = self.sim.get_state()

        self.sim.set_state(sim_state0)
        self.sim.settle(max_steps=1000)
        sim_state1 = self.sim.get_state()

        rope = [
            sim_obj for sim_obj in self.sim.sim_objs
            if isinstance(sim_obj, RopeSimulationObject)
        ][0]
        box = BoxSimulationObject("box", [0] * 3, [.1] * 3, dynamic=False)

        self.sim.remove_objects([rope])
        self.sim.set_state(sim_state0)
        self.sim.settle(max_steps=1000)
        sim_state2 = self.sim.get_state(
        )  # this adds another rope that has the same properties as rope

        self.sim.add_objects([box])
        self.sim.set_state(sim_state0)
        self.sim.settle(max_steps=1000)
        sim_state3 = self.sim.get_state(
        )  # this removes the recently added box

        rope = [
            sim_obj for sim_obj in self.sim.sim_objs
            if isinstance(sim_obj, RopeSimulationObject)
        ][0]

        self.sim.remove_objects([rope])
        self.sim.add_objects([box])
        self.sim.set_state(sim_state0)
        self.sim.settle(max_steps=1000)
        sim_state4 = self.sim.get_state()

        self.assertArrayDictEqual(sim_state1[1], sim_state2[1])
        self.assertArrayDictEqual(sim_state1[1], sim_state3[1])
        self.assertArrayDictEqual(sim_state1[1], sim_state4[1])

    def assertArrayDictEqual(self, d0, d1):
        self.assertSetEqual(set(d0.keys()), set(d1.keys()))
        for (k, v0) in d0.iteritems():
            v1 = d1[k]
            self.assertTrue(np.all(v0 == v1))