Mobile bases added. (stepjam#27)

* -1 instead of None for making object orphan

* init mobile robot support

* youBot arm added

* added some lua functions for mobile motion planning

* added child script to mobile scenes to allow  appropriate suffix when multiple instance

* base_ref needs to be parent of the robot tree when computing path for collision check

* added dubins params for smoother trajectories

* updated dummy scene

* description example changed

* changed args order init

* runtimerror when path completed, corrected height target

* controller two_wheels distance dependancy added, corrected height target, description info

* lowered threshold omni robot

* remove output function

* Changes to make tests pass.

* Refactor of mobile platforms.

* Update to youbot model file.

* Added packages to setup script.

* Gn3112 mobile base (stepjam#24)

* added algorithm option to mobile base

* added path checking for get_linear mobile base

* added comments

* docstring corrections

* added algorithm arg, correction docstrings

* deleted comment

* moved waypoint causing test to fail

* Fixed bugs in mobiles. Added PID for nonholonomic. Added LoCoBot.

* Renaming examples. Joints no longer call get/set config tree.

* Model modifications.

README.md

@@ -205,11 +205,16 @@ Here is a list of robots currently supported by PyRep:
 - Rethink Baxter Gripper
 - Franka Emika Panda Gripper
 
+#### Mobile Robots
+
+- Kuka YouBot
+- Turtle Bot
+- Line Tracer
+
 Feel free to send pull requests for new robots!
 
 ## Planned Future Updates
 
-- Support for mobile bases (including planning)
 - Support for MuJoCo
 - Sim-to-Real support (e.g. domain randomization)
 
@@ -227,6 +232,10 @@ If you use PyRep in your work, then get in contact and we can add you to the lis
 - [Transferring End-to-End Visuomotor Control from Simulation to Real World for a Multi-Stage Task
 , CoRL 2017](https://arxiv.org/abs/1707.02267)
 
+## Acknowledgements
+
+- Georges Nomicos (Imperial College London) for the addition of mobile platforms.
+
 ## Citation
 
 ```

cffi_build/cffi_build.py

@@ -1,6 +1,7 @@
 from cffi import FFI
 import os
 import glob
+from shutil import copyfile
 
 # Get PYREP root and find the needed files to compile the cffi lib.
 
@@ -730,5 +731,11 @@
     print('creating symlink: %s -> %s' % (path + '.1', path))
     os.symlink(path, path + '.1')
 
+# Copy lua functions to the VREP_ROOT
+print('copying lua file: %s -> %s' % ('pyrep/backend', os.environ['VREP_ROOT']))
+lua_script_fname = 'vrepAddOnScript_PyRep.lua'
+copyfile(os.path.join('pyrep/backend', lua_script_fname),
+         os.path.join(os.environ['VREP_ROOT'], lua_script_fname))
+
 if __name__ == "__main__":
     ffibuilder.compile(verbose=True)

examples/example_baxter_pick_and_pass.py

@@ -12,7 +12,7 @@
 from pyrep.objects.dummy import Dummy
 from pyrep.objects.shape import Shape
 
-SCENE_FILE = join(dirname(abspath(__file__)), 'baxter_pick_and_pass.ttt')
+SCENE_FILE = join(dirname(abspath(__file__)), 'scene_baxter_pick_and_pass.ttt')
 pr = PyRep()
 
 pr.launch(SCENE_FILE, headless=False)

examples/example_locobot_stack_cube.py

@@ -0,0 +1,96 @@
+"""
+Facebook/CMU LoCoBot drives to a cube, picks it up, and takes it to a target.
+This script contains examples of:
+    - Linear mobile paths on a non-holonomic platform
+    - How to use the combination of a mobile base, manipulator, and gripper.
+"""
+from os.path import dirname, join, abspath
+from pyrep import PyRep
+from pyrep.robots.mobiles.locobot import LoCoBot
+from pyrep.robots.arms.locobot_arm import LoCoBotArm
+from pyrep.robots.end_effectors.locobot_gripper import LoCoBotGripper
+from pyrep.objects.shape import Shape
+from pyrep.objects.dummy import Dummy
+
+SCENE_FILE = join(dirname(abspath(__file__)), 'scene_locobot_stack_cube.ttt')
+pr = PyRep()
+pr.launch(SCENE_FILE, headless=False)
+pr.start()
+
+base = LoCoBot()
+arm = LoCoBotArm()
+gripper = LoCoBotGripper()
+
+base.set_motor_locked_at_zero_velocity(True)
+
+
+def drive_to_position(position, orientation):
+    base_path = base.get_linear_path(position, orientation)
+    base_path.visualize()
+    done = False
+    while not done:
+        done = base_path.step()
+        pr.step()
+    pr.step()
+
+
+def move_arm(position, quaternion, ignore_collisions=False):
+    arm_path = arm.get_path(position,
+                            quaternion=quaternion,
+                            ignore_collisions=ignore_collisions)
+    arm_path.visualize()
+    done = False
+    while not done:
+        done = arm_path.step()
+        pr.step()
+    arm_path.clear_visualization()
+
+
+cuboid = Shape('cuboid')
+goal = Shape('goal')
+grasp_point = Dummy('grasp_point')
+
+drive_pos = cuboid.get_position()
+drive_pos[1] -= 0.3
+
+print('Driving to cube ...')
+drive_to_position(drive_pos, 0)
+
+grasp_point_raised = grasp_point.get_position()
+grasp_point_raised[2] += 0.075
+
+print('Move arm above cube ...')
+move_arm(grasp_point_raised, grasp_point.get_quaternion())
+
+print('Arm approach cube ...')
+move_arm(grasp_point.get_position(), grasp_point.get_quaternion(), True)
+
+print('Closing gripper ...')
+while not gripper.actuate(0.0, 0.4):
+    pr.step()
+gripper.grasp(cuboid)
+
+print('Lift cube ...')
+move_arm(grasp_point_raised, grasp_point.get_quaternion(), True)
+
+drive_pos = goal.get_position()
+drive_pos[1] -= 0.35
+
+print('Driving to goal ...')
+drive_to_position(drive_pos, 0)
+
+goal_point_raised = goal.get_position()
+goal_point_raised[2] += 0.05
+
+print('Move arm above goal ...')
+move_arm(goal_point_raised, grasp_point.get_quaternion())
+
+print('Drop cube ...')
+gripper.release()
+while not gripper.actuate(1.0, 0.4):
+    pr.step()
+
+print('Done!')
+
+pr.stop()
+pr.shutdown()

examples/example_panda_end_effector_control.py

@@ -8,7 +8,7 @@
 from pyrep import PyRep
 from pyrep.robots.arms.panda import Panda
 
-SCENE_FILE = join(dirname(abspath(__file__)), 'panda_reach_target.ttt')
+SCENE_FILE = join(dirname(abspath(__file__)), 'scene_panda_reach_target.ttt')
 DELTA = 0.01
 pr = PyRep()
 pr.launch(SCENE_FILE, headless=False)

examples/example_panda_reach_target.py

@@ -14,7 +14,7 @@
 import math
 
 LOOPS = 10
-SCENE_FILE = join(dirname(abspath(__file__)), 'panda_reach_target.ttt')
+SCENE_FILE = join(dirname(abspath(__file__)), 'scene_panda_reach_target.ttt')
 pr = PyRep()
 pr.launch(SCENE_FILE, headless=False)
 pr.start()

examples/example_reinforcement_learning_env.py

@@ -13,7 +13,8 @@
 from pyrep.objects.shape import Shape
 import numpy as np
 
-SCENE_FILE = join(dirname(abspath(__file__)), 'reinforcement_learning_env.ttt')
+SCENE_FILE = join(dirname(abspath(__file__)),
+                  'scene_reinforcement_learning_env.ttt')
 POS_MIN, POS_MAX = [0.8, -0.2, 1.0], [1.0, 0.2, 1.4]
 EPISODES = 5
 EPISODE_LENGTH = 200

examples/example_turtlebot_navigation.py

@@ -0,0 +1,53 @@
+"""
+A turtlebot reaches for 4 randomly places targets.
+This script contains examples of:
+    - Non-linear mobile paths to reach a target with collision avoidance
+"""
+from os.path import dirname, join, abspath
+from pyrep import PyRep
+from pyrep.robots.mobiles.turtlebot import TurtleBot
+from pyrep.objects.shape import Shape
+from pyrep.const import PrimitiveShape
+import numpy as np
+
+LOOPS = 4
+SCENE_FILE = join(dirname(abspath(__file__)), 'scene_turtlebot_navigation.ttt')
+pr = PyRep()
+pr.launch(SCENE_FILE, headless=False)
+pr.start()
+agent = TurtleBot()
+
+# We could have made this target in the scene, but lets create one dynamically
+target = Shape.create(type=PrimitiveShape.SPHERE,
+                      size=[0.05, 0.05, 0.05],
+                      color=[1.0, 0.1, 0.1],
+                      static=True, respondable=False)
+
+position_min, position_max = [-0.5, 1, 0.1], [0.5, 1.5, 0.1]
+
+starting_pose = agent.get_2d_pose()
+
+agent.set_motor_locked_at_zero_velocity(True)
+
+for i in range(LOOPS):
+    agent.set_2d_pose(starting_pose)
+
+    # Get a random position within a cuboid and set the target position
+    pos = list(np.random.uniform(position_min, position_max))
+    target.set_position(pos)
+
+    path = agent.get_nonlinear_path(position=pos, angle=0)
+
+    path.visualize()
+    done = False
+
+    while not done:
+        done = path.step()
+        pr.step()
+
+    path.clear_visualization()
+
+    print('Reached target %d!' % i)
+
+pr.stop()
+pr.shutdown()

examples/example_youbot_navigation.py

@@ -0,0 +1,51 @@
+"""
+A Kuka youBot reaches for 4 randomly places targets.
+This script contains examples of:
+    - Linear mobile paths with an omnidirectional robot to reach a target.
+"""
+from os.path import dirname, join, abspath
+from pyrep import PyRep
+from pyrep.robots.mobiles.youbot import YouBot
+from pyrep.objects.shape import Shape
+from pyrep.const import PrimitiveShape
+import numpy as np
+
+LOOPS = 4
+SCENE_FILE = join(dirname(abspath(__file__)), 'scene_youbot_navigation.ttt')
+pr = PyRep()
+pr.launch(SCENE_FILE, headless=False)
+pr.start()
+agent = YouBot()
+
+# We could have made this target in the scene, but lets create one dynamically
+target = Shape.create(type=PrimitiveShape.SPHERE,
+                      size=[0.05, 0.05, 0.05],
+                      color=[1.0, 0.1, 0.1],
+                      static=True, respondable=False)
+
+position_min, position_max = [-0.5, 1.4, 0.1], [1.0, 0.5, 0.1]
+
+starting_pose = agent.get_2d_pose()
+
+for i in range(LOOPS):
+    agent.set_2d_pose(starting_pose)
+
+    # Get a random position within a cuboid and set the target position
+    pos = list(np.random.uniform(position_min, position_max))
+    target.set_position(pos)
+
+    path = agent.get_linear_path(position=pos, angle=0)
+
+    path.visualize()
+
+    done = False
+    while not done:
+        done = path.step()
+        pr.step()
+
+    path.clear_visualization()
+
+    print('Reached target %d!' % i)
+
+pr.stop()
+pr.shutdown()