From 557c4620df66e3dbdeb121dba2ddf2c1017be1cb Mon Sep 17 00:00:00 2001 From: Brian Delhaisse Date: Fri, 12 Jul 2019 12:20:06 +0200 Subject: [PATCH] update worlds --- .../worlds/samples/sports/basketball.py | 17 +++++++- pyrobolearn/worlds/samples/sports/billiard.py | 16 +++++++- pyrobolearn/worlds/samples/sports/darts.py | 15 ++++++- .../worlds/samples/sports/ping_pong.py | 34 ++++++++++++++- pyrobolearn/worlds/samples/sports/tennis.py | 41 +++++++++++++++++-- pyrobolearn/worlds/world.py | 2 + 6 files changed, 117 insertions(+), 8 deletions(-) diff --git a/pyrobolearn/worlds/samples/sports/basketball.py b/pyrobolearn/worlds/samples/sports/basketball.py index 2f81460..9d1ed77 100644 --- a/pyrobolearn/worlds/samples/sports/basketball.py +++ b/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) diff --git a/pyrobolearn/worlds/samples/sports/billiard.py b/pyrobolearn/worlds/samples/sports/billiard.py index 1d99b11..3a9b105 100644 --- a/pyrobolearn/worlds/samples/sports/billiard.py +++ b/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) diff --git a/pyrobolearn/worlds/samples/sports/darts.py b/pyrobolearn/worlds/samples/sports/darts.py index 761b35c..6e489ba 100644 --- a/pyrobolearn/worlds/samples/sports/darts.py +++ b/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) diff --git a/pyrobolearn/worlds/samples/sports/ping_pong.py b/pyrobolearn/worlds/samples/sports/ping_pong.py index b62bf3c..1087240 100644 --- a/pyrobolearn/worlds/samples/sports/ping_pong.py +++ b/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) diff --git a/pyrobolearn/worlds/samples/sports/tennis.py b/pyrobolearn/worlds/samples/sports/tennis.py index 041a0ce..584443e 100644 --- a/pyrobolearn/worlds/samples/sports/tennis.py +++ b/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) diff --git a/pyrobolearn/worlds/world.py b/pyrobolearn/worlds/world.py index 4f5c5b2..f6044c5 100644 --- a/pyrobolearn/worlds/world.py +++ b/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,