wassname/pyrobolearn
1
0
Fork 0
mirror of https://github.com/wassname/pyrobolearn.git synced 2026-09-13 12:50:44 +08:00
Code Issues Packages Projects Releases Wiki Activity

add 2 examples with world camera + corresponding updates

Browse Source
This commit is contained in:
Brian Delhaisse
2019-06-19 16:54:02 +02:00
parent 687619037e
commit 1f4764d923
9 changed files with 241 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files
examples/kinematics/README.md
+13
View File
@@ -2,6 +2,19 @@
We provide examples on how to perform forward and inverse kinematics.
Here are the forward kinematics (FK) examples that the user can try:
1. `fk.py`: simple forward kinematics example where we directly sent desired joint positions to the Kuka
manipulator.
Here are the inverse kinematics (IK) examples that the user can try:
1. `ik.py`: simple inverse kinematics example where the Kuka manipulator has to reach a certain target position
in the world. In this example, the user can also choose the damped-least-squares IK solver.
2. `ik_libraries.py`: comparison between different IK libraries including `pybullet`, `PyKDL`, `trac_ik`, and
`rbdl` using the Kuka manipulator.
3. `moving_sphere.py`: damped-least-squares IK with the Kuka manipulator where the goal is to follow a sphere
that moves in a circular manner.
References:
- [1] "Robotics: Modelling, Planning and Control", Siciliano et al., 2010
- [2] "Springer Handbook of Robotics", Siciliano et al., 2008
examples/kinematics/inverse/ik.py
+14 -5
View File
@@ -8,16 +8,25 @@ Set the `solver_flag` to a number between 0 and 1 (see lines [19,22]) to select
import numpy as np
from itertools import count
import argparse
from pyrobolearn.simulators import Bullet
from pyrobolearn.worlds import BasicWorld
from pyrobolearn.robots import KukaIIWA
# create parser to select the IK solver
parser = argparse.ArgumentParser()
parser.add_argument('-s', '--solver', help='the IK solver to select (0: use robot.calculate_inverse_kinematics(), '
'1: use damped-least-squares IK using Jacobian)', type=int,
choices=[0, 1], default=1)
args = parser.parse_args()
# select IK solver, by setting the flag:
# 0 = pybullet + calculate_inverse_kinematics()
# 1 = pybullet + damped-least-squares IK using Jacobian (provided by pybullet)
solver_flag = 1 # 1 and 4 gives pretty good results
solver_flag = args.solver # 1 gives a pretty good result
# Create simulator
@@ -34,6 +43,7 @@ robot.print_info()
dt = 1./240
link_id = robot.get_end_effector_ids(end_effector=0)
joint_ids = robot.joints # actuated joint
# joint_ids = joint_ids[2:]
damping = 0.01 # for damped-least-squares IK
wrt_link_id = -1 # robot.get_link_ids('iiwa_link_1')
@@ -41,11 +51,10 @@ wrt_link_id = -1 # robot.get_link_ids('iiwa_link_1')
xd = np.array([0.5, 0., 0.5])
world.load_visual_sphere(xd, radius=0.05, color=(1, 0, 0, 0.5))
# joint_ids = joint_ids[2:]
# change the robot visual
robot.change_transparency()
robot.draw_link_frames([-1, 0])
robot.draw_bounding_boxes(joint_ids[0])
robot.draw_link_frames(link_ids=[-1, 0])
robot.draw_bounding_boxes(link_ids=joint_ids[0])
# robot.draw_link_coms([-1,0])
qIdx = robot.get_q_indices(joint_ids)
examples/kinematics/inverse/ik_libraries.py
+46 -31
View File
@@ -14,40 +14,23 @@ Set the `solver_flag` to a number between 0 and 4 (see lines [53,60]) to select
import os
import numpy as np
from itertools import count
import argparse
from pyrobolearn.simulators import Bullet
from pyrobolearn.worlds import BasicWorld
from pyrobolearn.robots import KukaIIWA
# import PyKDL
try:
import PyKDL as kdl
except ImportError as e:
raise ImportError(repr(e) + '\nTry to install `PyKDL`: '
'sudo apt-get install ros-<distribution>-python-orocos-kdl'
'or install it manually from `https://github.com/orocos/orocos_kinematics_dynamics`')
# import kdl_parser_py
try:
import kdl_parser_py.urdf as KDLParser
except ImportError as e:
raise ImportError(repr(e) + '\nTry to install `kdl_parser_py`: '
'sudo apt-get install ros-<distribution>-kdl-parser-py')
# import track_ik_python
try:
from trac_ik_python.trac_ik import IK as TracIK
except ImportError as e:
raise ImportError(repr(e) + '\nTry to install `trac_ik_python`: '
'sudo apt-get install ros-<distribution>-trac-ik-python')
# import rbdl
try:
import rbdl
except ImportError as e:
raise ImportError(repr(e) + '\nTry to install `rbdl` manually from `https://bitbucket.org/rbdl/rbdl`')
# create parser to select the IK solver
parser = argparse.ArgumentParser()
parser.add_argument('-s', '--solver', help='the IK solver to select:\n'
'0: use robot.calculate_inverse_kinematics()\n'
'1: use damped-least-squares IK using Jacobian (provided by simulator)\n'
'2: use PyKDL\n'
'3: use trac_ik\n'
'4: use rbdl + damped-least-squares IK using Jacobian (provided by rbdl)',
type=int, choices=[0, 1, 2, 3, 4], default=1)
args = parser.parse_args()
# TO BE SET BY THE USER
# select IK solver, by setting the flag:
@@ -56,7 +39,39 @@ except ImportError as e:
# 2 = PyKDL
# 3 = trac_ik
# 4 = rbdl + damped-least-squares IK using Jacobian (provided by rbdl)
solver_flag = 1 # 1 and 4 gives pretty good results
solver_flag = args.solver # 1 and 4 gives pretty good results
if solver_flag == 2: # PyKDL
# import PyKDL
try:
import PyKDL as kdl
except ImportError as e:
raise ImportError(repr(e) + '\nTry to install `PyKDL`: '
'sudo apt-get install ros-<distribution>-python-orocos-kdl'
'or install it manually from '
'`https://github.com/orocos/orocos_kinematics_dynamics`')
# import kdl_parser_py
try:
import kdl_parser_py.urdf as kdl_parser
except ImportError as e:
raise ImportError(repr(e) + '\nTry to install `kdl_parser_py`: '
'sudo apt-get install ros-<distribution>-kdl-parser-py')
elif solver_flag == 3: # trac_ik_python
# import trac_ik_python
try:
from trac_ik_python.trac_ik import IK as trac_ik
except ImportError as e:
raise ImportError(repr(e) + '\nTry to install `trac_ik_python`: '
'sudo apt-get install ros-<distribution>-trac-ik-python')
elif solver_flag == 4: # rbdl
# import rbdl
try:
import rbdl
except ImportError as e:
raise ImportError(repr(e) + '\nTry to install `rbdl` manually from `https://bitbucket.org/rbdl/rbdl`')
# Create simulator
@@ -181,7 +196,7 @@ elif solver_flag == 1:
##################
elif solver_flag == 2:
print("Using PyKDL:")
model = KDLParser.treeFromFile(urdf)
model = kdl_parser.treeFromFile(urdf)
if model[0]:
model = model[1]
else:
@@ -237,7 +252,7 @@ elif solver_flag == 3:
urdf_string = open(urdf, 'r').read()
# create IK solver
ik_solver = TracIK(base_link=base_name, tip_link=end_effector_name, urdf_string=urdf_string, solve_type='Distance')
ik_solver = trac_ik(base_link=base_name, tip_link=end_effector_name, urdf_string=urdf_string, solve_type='Distance')
# define upper and lower limits (optional)
# lb, ub = -np.ones(6)*100, np.ones(6)*100
examples/worlds/README.md
+5 -3
View File
@@ -9,6 +9,8 @@ The world is usually created once the simulator has been selected.
Here are the examples that the user can try:
1. `load_world.py`: load a basic world (i.e. with a floor and gravity enabled) with different objects (only visual,
and with collisions) that are movable, fixed, or are moving.
2. `load_robot.py`: load a robot in a basic world and distribute randomly few objects on the floor.
3. `load_heightmap.py`: load a terrain from a heightmap (png) and load a robot on it.
4. `generate_terrain.py`: generate a terrain and distribute randomly few objects on the terrain.
2. `follow_moving_body.py`: follow a moving body with the world camera.
3. `move_camera.py`: get the world camera and move it in the world using the keyboard interface.
4. `load_robot.py`: load a robot in a basic world and distribute randomly few objects on the floor.
5. `load_heightmap.py`: load a terrain from a heightmap (png) and load a robot on it.
6. `generate_terrain.py`: generate a terrain and distribute randomly few objects on the terrain.
examples/worlds/follow_moving_body.py
+25
View File
@@ -0,0 +1,25 @@
#!/usr/bin/env python
"""Follow a body with the main camera in the world.
Try to move the sphere with the mouse (left-click on the object).
"""
from itertools import count
import pyrobolearn as prl
# create simulator
sim = prl.simulators.Bullet()
# create basic world (with a floor and gravity enabled by default)
world = prl.worlds.BasicWorld(sim)
# load sphere
sphere = world.load_sphere(position=[0, 0, 15.], radius=0.2, mass=1, color=(1, 0, 0, 1))
# run simulator
for _ in count():
# follow sphere
world.follow(sphere)
# perform one step in the world
world.step(sleep_dt=1. / 240)
examples/worlds/move_camera.py
+58
View File
@@ -0,0 +1,58 @@
#!/usr/bin/env python
"""Move main camera in the world.
Move the main camera in the world using the keyboard:
- top arrow: move forward
- bottom arrow: move backward
- left arrow: move sideways to the left
- right arrow: move sideways to the right
- ctrl + top arrow: turn downward
- ctrl + bottom arrow: turn upward
- ctrl + left arrow: turn to the right
- ctrl + right arrow: turn to the left
"""
from itertools import count
import pyrobolearn as prl
# create simulator
sim = prl.simulators.Bullet()
# create basic world (with a floor and gravity enabled by default)
world = prl.worlds.BasicWorld(sim)
# get world camera
camera = world.camera
# create mouse-keyboard interface
interface = prl.tools.interfaces.MouseKeyboardInterface(sim)
# run simulator
for _ in count():
# perform a step with the interface (i.e. get events from interface)
interface.step()
# check the keys that are down
key, key_down = interface.key, interface.key_down
if key.ctrl in key_down:
if key.top_arrow in key_down:
camera.pitch -= 0.005 # turn downward
elif key.bottom_arrow in key_down:
camera.pitch += 0.005 # turn upward
elif key.left_arrow in key_down:
camera.yaw -= 0.005 # turn to the right
elif key.right_arrow in key_down:
camera.yaw += 0.005 # turn to the left
else:
if key.top_arrow in key_down:
camera.target_position += 0.01 * camera.forward_vector # move forward
elif key.bottom_arrow in key_down:
camera.target_position -= 0.01 * camera.forward_vector # move backward
elif key.left_arrow in key_down:
camera.target_position -= 0.01 * camera.lateral_vector # move to the left
elif key.right_arrow in key_down:
camera.target_position += 0.01 * camera.lateral_vector # move to the right
# perform one step in the world
world.step(sleep_dt=1. / 254)