Commit 5f56e60b authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

Update script simple-robot.py to hpp version 3.

parent 2bf2cded
#/usr/bin/env python
import numpy as np
import matplotlib.pyplot as pl
from hpp.corbaserver import Client
from hpp_corbaserver.hpp import Configuration
......@@ -7,21 +9,45 @@ from hpp_corbaserver.hpp import Configuration
cfg1 = Configuration (rot = (1,0,0,0,1,0,0,0,1), trs = (0,0,0))
cl = Client ()
cl.robot.createRobot ('robot')
bounds = [-5., 5., -5., 5., -.01, .01, 1., -1., 1., -1., 1., -1.]
cl.robot.createJoint ('J1', 'freeflyer', cfg1, bounds)
cl.robot.setRobotRootJoint ('robot', 'J1')
bounds = [-5., 5.]
cl.robot.createJoint ('Tx', 'translation', cfg1, bounds)
cl.robot.setRobotRootJoint ('robot', 'Tx')
cfg2 = Configuration (rot = (0, -1, 0, 1, 0, 0, 0, 0, 1), trs = (0,0,0))
cl.robot.createJoint ('Ty', 'translation', cfg2, bounds)
cl.robot.addJoint ('Tx', 'Ty')
cl.robot.createSphere ("link", 1.)
cl.robot.addObjectToJoint ("J1", "link", cfg1)
cl.robot.addObjectToJoint ("Ty", "link", cfg1)
cl.robot.setRobot ('robot')
# Create obstacles
cl.obstacle.createBox ("obst1", 1., 1., 0.2)
cl.obstacle.addObstacle ("obst1", True, True)
#cl.obstacle.addObstacle ("obst1", True, True)
init = [-2., 0., 0., 0., 0., 0.]
goal = [2.,0., 0., 0., 0., 0.]
init = [-2., 0.]
goal = [2.,0.]
cl.problem.setInitialConfig (init)
cl.problem.addGoalConfig (goal)
cl.problem.solve ()
if cl.problem.solve () != 0:
raise RuntimeError ("failed to solve the problem")
n = cl.problem.numberPaths () -1
if n < 0:
raise RuntimeError ("Problem solved successfully, but no resulting path")
x = np.array ([])
y = np.array ([])
length = cl.problem.pathLength (n)
dt = 1e-2
nbSamples = int (length/dt + 1)
for i in range (nbSamples):
t = dt*i
q = cl.problem.configAtDistance (n, t)
x = np.append (x, q [0])
y = np.append (y, q [1])
fig = pl.figure ()
ax = fig.add_subplot ('111')
ax.plot (x,y)
pl.show ()
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment