update worlds

pyrobolearn/worlds/samples/sports/basketball.py

@@ -18,6 +18,8 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
+# TODO: finish to implement the world, create corresponding environment (in `envs` folder) with state and reward.
+
 class BasketBallWorld(BasicWorld):
     r"""Basketball world
 
@@ -43,7 +45,9 @@ class BasketBallWorld(BasicWorld):
         # load ball
         # self.ball = self.load_sphere(position=[0., 0., 1.], radius=0.1193, mass=0.625)
         # self.apply_texture(texture=mesh_path + 'Basketball-ColorMap.jpg', body_id=self.ball)
-        self.ball = self.load_mesh(mesh_path + 'ball.obj', position=[0., 0., 1.], scale=scale, mass=0.625, flags=0)
+        self.ball = self.load_mesh(mesh_path + 'ball.obj', position=[0., 0., 2.], scale=(1., 1., 1.), mass=0.625,
+                                   flags=0)
+        self.ball_radius = 0.1193
 
         # set the restitution coefficient for the ball
         # Ref: "Measure the coefficient of restitution for sports balls", Persson, 2012
@@ -62,9 +66,20 @@ if __name__ == '__main__':
     from itertools import count
     import pyrobolearn as prl
 
+    # create simulator
     sim = prl.simulators.Bullet()
 
+    # create world
     world = BasketBallWorld(sim)
 
+    # create manipulator
+    robot = world.load_robot('kuka_iiwa')
+
+    # attach ball to robot end effector
+    world.attach(body1=robot, body2=world.ball, link1=robot.end_effectors[0], link2=-1, joint_axis=[0., 0., 0.],
+                 parent_frame_position=[0., 0., world.ball_radius], child_frame_position=[0., 0., 0.],
+                 parent_frame_orientation=[0, 0., 0., 1.])
+
+    # run simulation
     for t in count():
        world.step(sim.dt)

pyrobolearn/worlds/samples/sports/billiard.py

@@ -18,6 +18,8 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
+# TODO: finish to implement the world, create corresponding environment (in `envs` folder) with state and reward.
+
 class BilliardWorld(BasicWorld):
     r"""Billiard world
 
@@ -46,7 +48,7 @@ class BilliardWorld(BasicWorld):
 
         # load cue
         self.cue1 = self.load_mesh(mesh_path + 'cue.obj', position=position + np.array([-0.5, 0.4, 0.4]), mass=0.595,
-                                   scale=(1.5, 1., 1.), flags=0, return_body=True)
+                                   scale=(1., 1., 1.), flags=0, return_body=True)
 
         # load balls
         # the order is based on: https://www.wikihow.com/Rack-a-Pool-Table
@@ -94,9 +96,21 @@ if __name__ == '__main__':
     from itertools import count
     import pyrobolearn as prl
 
+    # create simulator
     sim = prl.simulators.Bullet()
 
+    # create world
     world = BilliardWorld(sim)
 
+    # create manipulator
+    robot = world.load_robot('kuka_iiwa', position=[-2., 0.2, 0.])
+
+    # attach cue to robot end effector
+    # Note that you can detach the cue from the robot end effector using `world.detach`
+    world.attach(body1=robot, body2=world.cue1, link1=robot.end_effectors[0], link2=-1, joint_axis=[0., 0., 0.],
+                 parent_frame_position=[-0., 0., 0.02], child_frame_position=[0., 0., 0.],
+                 parent_frame_orientation=[0, 0., 0., 1.])
+
+    # run simulation
     for t in count():
         world.step(sim.dt)

pyrobolearn/worlds/samples/sports/darts.py

@@ -18,6 +18,8 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
+# TODO: finish to implement the world, create corresponding environment (in `envs` folder) with state and reward.
+
 class DartsWorld(BasicWorld):
     r"""Darts world
 
@@ -68,9 +70,20 @@ if __name__ == '__main__':
     from itertools import count
     import pyrobolearn as prl
 
+    # create simulator
     sim = prl.simulators.Bullet()
 
+    # create world
     world = DartsWorld(sim)
 
+    # create manipulator
+    robot = world.load_robot('kuka_iiwa')
+
+    # attach first dart to robot end effector
+    world.attach(body1=robot, body2=world.darts[0], link1=robot.end_effectors[0], link2=-1, joint_axis=[0., 0., 0.],
+                 parent_frame_position=[0., 0., 0.02], child_frame_position=[0., 0., 0.],
+                 parent_frame_orientation=[0, 0., 0., 1.])
+
+    # run simulation
     for t in count():
-       world.step(sim.dt)
+        world.step(sim.dt)

pyrobolearn/worlds/samples/sports/ping_pong.py

@@ -18,6 +18,8 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
+# TODO: finish to implement the world, create corresponding environment (in `envs` folder) with state and reward.
+
 class PingPongWorld(BasicWorld):
     r"""Ping Pong world
 
@@ -113,10 +115,40 @@ if __name__ == '__main__':
     from itertools import count
     import pyrobolearn as prl
 
+    # create simulator
     sim = prl.simulators.Bullet()
 
+    # load world
     world = PingPongWorld(sim)
-    # world = BallOnPaddleWorld(sim)
 
+    # Tests before creating environment
+    # load 2 robots
+    robot1 = world.load_robot('kuka_iiwa', position=[1.8, 0., 0.2], fixed_base=True)
+    robot2 = world.load_robot('kuka_iiwa', position=[-1.8, 0., 0.2], fixed_base=True)
+
+    # attach each paddle to the robot's end-effector
+    world.attach(body1=robot1, body2=world.paddle1, link1=robot1.end_effectors[0], link2=-1, joint_axis=[0., 0., 0.],
+                 parent_frame_position=[0., 0., 0.02], child_frame_position=[0., 0., 0.],
+                 parent_frame_orientation=[0, -0.707, 0, 0.707])
+    world.attach(body1=robot2, body2=world.paddle2, link1=robot2.end_effectors[0], link2=-1, joint_axis=[0., 0., 0.],
+                 parent_frame_position=[0., 0., 0.02], child_frame_position=[0., 0., 0.],
+                 parent_frame_orientation=[0, 0.707, 0, 0.707])
+
+    # run the simulation
+    direction = 1
+    dy = np.array([0., 0.005, 0.])
+    y_lim = 0.8
     for t in count():
+        # move the robot base (only valid in the simulator)
+        x, y, z = robot1.position
+        if y > y_lim:
+            robot1.position -= dy
+            direction = -1
+        elif y < -y_lim and direction == -1:
+            robot1.position += dy
+            direction = 1
+        else:
+            robot1.position += direction * dy
+
+        # step in the simulation
         world.step(sim.dt)

pyrobolearn/worlds/samples/sports/tennis.py

@@ -20,6 +20,8 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
+# TODO: finish to implement the world, create corresponding environment (in `envs` folder) with state and reward.
+
 class TennisWorld(BasicWorld):
     r"""Tennis world
 
@@ -36,10 +38,10 @@ class TennisWorld(BasicWorld):
         net = self.load_mesh(mesh_path + 'net.obj', position=position, scale=scale, mass=0, flags=1)
 
         # load rackets
-        self.racket1 = self.load_mesh(mesh_path + 'racket.obj', position=(-6., 0., 1.), scale=scale, mass=0, flags=0,
-                                      return_body=True)
-        self.racket2 = self.load_mesh(mesh_path + 'racket.obj', position=(6., 0., 1.), scale=scale, mass=0, flags=0,
-                                      return_body=True)
+        self.racket1 = self.load_mesh(mesh_path + 'racket.obj', position=(-6., 0., 1.), scale=scale, mass=0.260,
+                                      flags=0, return_body=True)
+        self.racket2 = self.load_mesh(mesh_path + 'racket.obj', position=(6., 0., 1.), scale=scale, mass=0.260,
+                                      flags=0, return_body=True)
 
         # load ball
         self.ball = self.load_mesh(mesh_path + 'ball.obj', position=(2, 0., 1.), scale=scale, mass=0.0585, flags=0,
@@ -62,9 +64,40 @@ if __name__ == '__main__':
     from itertools import count
     import pyrobolearn as prl
 
+    # create simulator
     sim = prl.simulators.Bullet()
 
+    # create world
     world = TennisWorld(sim)
 
+    # Tests before creating environment
+    # load 2 robots
+    robot1 = world.load_robot('kuka_iiwa', position=[5, 0., 0.2], fixed_base=True)
+    robot2 = world.load_robot('kuka_iiwa', position=[-5, 0., 0.2], fixed_base=True)
+
+    # attach each paddle to the robot's end-effector
+    world.attach(body1=robot1, body2=world.racket1, link1=robot1.end_effectors[0], link2=-1, joint_axis=[0., 0., 0.],
+                 parent_frame_position=[0., 0., 0.03], child_frame_position=[0., 0., -0.3],
+                 parent_frame_orientation=[0, -0.707, 0, 0.707])
+    world.attach(body1=robot2, body2=world.racket2, link1=robot2.end_effectors[0], link2=-1, joint_axis=[0., 0., 0.],
+                 parent_frame_position=[0., 0., 0.03], child_frame_position=[0., 0., -0.3],
+                 parent_frame_orientation=[0, 0.707, 0, 0.707])
+
+    # run simulation
+    direction = 1
+    dy = np.array([0., 0.005, 0.])
+    y_lim = 2.
     for t in count():
+        # move the robot base (only valid in the simulator)
+        x, y, z = robot1.position
+        if y > y_lim:
+            robot1.position -= dy
+            direction = -1
+        elif y < -y_lim and direction == -1:
+            robot1.position += dy
+            direction = 1
+        else:
+            robot1.position += direction * dy
+
+        # perform a step in the simulation
         world.step(sim.dt)

pyrobolearn/worlds/world.py

@@ -2039,6 +2039,8 @@ class World(object):
         """
         if body_id is None:
             body_id = self.floor_id
+        elif isinstance(body_id, Body):
+            body_id = body_id.id
         self.sim.change_dynamics(body_id=body_id, link_id=-1, lateral_friction=lateral_friction,
                                  spinning_friction=spinning_friction, rolling_friction=rolling_friction,
                                  restitution=restitution, linear_damping=linear_damping,