Пример #1
0
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)
sim.viewer.Idle()
Пример #2
0
                             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)
sim.viewer.Idle()
Пример #3
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))
Пример #4
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))