Skip to content

Commit

Permalink
change install usage to local usage
Browse files Browse the repository at this point in the history
  • Loading branch information
liuzhy71 committed Jul 14, 2021
1 parent adcedb1 commit f2146e0
Show file tree
Hide file tree
Showing 2 changed files with 51 additions and 48 deletions.
12 changes: 6 additions & 6 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -100,7 +100,7 @@ The best way to see how PyRep can help in your research is to look at the exampl
#### Launching the simulation

```python
from pyrep import PyRep
from PyRep.pyrep import PyRep

pr = PyRep()
# Launch the application with a scene file in headless mode
Expand All @@ -117,8 +117,8 @@ pr.shutdown() # Close the application
#### Modifying the Scene

```python
from pyrep.objects.shape import Shape
from pyrep.const import PrimitiveShape
from PyRep.pyrep.objects.shape import Shape
from PyRep.pyrep.const import PrimitiveShape

object = Shape.create(type=PrimitiveShape.CYLINDER,
color=[r,g,b], size=[w, h, d],
Expand All @@ -135,9 +135,9 @@ Use the robot ttm files defined in robots/ttms. These have been altered slightly
The 'tip' of the robot may not be where you want it, so feel free to play around with this.

```python
from pyrep import PyRep
from pyrep.robots.arms.panda import Panda
from pyrep.robots.end_effectors.panda_gripper import PandaGripper
from PyRep.pyrep import PyRep
from PyRep.pyrep.robots.arms.panda import Panda
from PyRep.pyrep.robots.end_effectors.panda_gripper import PandaGripper

pr = PyRep()
# Launch the application with a scene file that contains a robot
Expand Down
87 changes: 45 additions & 42 deletions examples/parallel_manipulator/env_docking_4PPPS.py
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,6 @@

logger = logging.getLogger(__name__)


SCENE_FILE = join(dirname(abspath(__file__)), 'cabin_docking_20210713_widergap.ttt')

# 固定藏段可以做位置调整
Expand All @@ -40,14 +39,14 @@ def __init__(self):
self.agent.set_motor_locked_at_zero_velocity(False)
self.stationary_cabin = Shape('cabin_static_front_phy')
self.stationary_cabin_tip = Dummy('cabin_static_face_center')
self.agent_ee_tip = self.agent.get_tip()
self.agent_ee_tip = Dummy('PM_4PPPS_front_center')
self.initial_joint_positions = self.agent.get_joint_positions()

def _get_state(self):
# 获取舱段中心点的位置、速度,以及视觉触感器获取的图像
return np.concatenate([self.agent.get_joint_positions(),
self.agent.get_joint_velocities(),
self.stationary_cabin_tip.get_position()])
self.agent_ee_tip.get_position()])

def reset(self):
# 设置舱段初始位姿,随机给定
Expand Down Expand Up @@ -81,42 +80,46 @@ def learn(self, replay_buffer):
pass


pr = PyRep()
pr.launch(SCENE_FILE, headless=False)
# pr.set_simulation_timestep(0.0001)
pr.start()
robot = PM_4PPPS()
waypoints = [Dummy('waypoint{}'.format(i+1)) for i in range(2)]
tip = Dummy('PM_4PPPS_front_center')

print('Planning path to the stationary cabin ...')
path = robot.get_path(position=waypoints[0].get_position(),
quaternion=waypoints[0].get_quaternion())
# path.visualize() # Let's see what the path looks like
# pr.step()
print('Executing plan ...')
done = False
while not done:
done = path.step()
pr.step()
logger.debug('distance: {}'.format(tip.get_pose()-waypoints[0].get_pose()))
# path.clear_visualization()

print('Insert the cabin ...')
path = robot.get_path(position=waypoints[1].get_position(),
quaternion=waypoints[1].get_quaternion())
# path.visualize() # Let's see what the path looks like
# pr.step()
print('Executing plan ...')
done = False
while not done:
done = path.step()
pr.step()
logger.debug('distance: {}'.format(tip.get_pose() - waypoints[1].get_pose()))
# path.clear_visualization()

print('Done ...')
input('Press enter to finish ...')
pr.stop()
pr.shutdown()

def manual_test():
pr = PyRep()
pr.launch(SCENE_FILE, headless=False)
# pr.set_simulation_timestep(0.0001)
pr.start()
robot = PM_4PPPS()
waypoints = [Dummy('waypoint{}'.format(i + 1)) for i in range(2)]
tip = Dummy('PM_4PPPS_front_center')

print('Planning path to the stationary cabin ...')
path = robot.get_path(position=waypoints[0].get_position(),
quaternion=waypoints[0].get_quaternion())
# path.visualize() # Let's see what the path looks like
# pr.step()
print('Executing plan ...')
done = False
while not done:
done = path.step()
pr.step()
logger.debug('distance: {}'.format(tip.get_pose() - waypoints[0].get_pose()))
# path.clear_visualization()

print('Insert the cabin ...')
path = robot.get_path(position=waypoints[1].get_position(),
quaternion=waypoints[1].get_quaternion())
# path.visualize() # Let's see what the path looks like
# pr.step()
print('Executing plan ...')
done = False
while not done:
done = path.step()
pr.step()
logger.debug('distance: {}'.format(tip.get_pose() - waypoints[1].get_pose()))
# path.clear_visualization()

print('Done ...')
input('Press enter to finish ...')
pr.stop()
pr.shutdown()


if __name__ == '__main__':
manual_test()

0 comments on commit f2146e0

Please sign in to comment.