test_rrt.py 2.4 KB
Newer Older
Florent Lamiraux's avatar
Florent Lamiraux committed
1
2
#/usr/bin/env python

3
from hpp.gepetto import Viewer, PathPlayer
4
from hpp.corbaserver.hrp2 import Robot
5
from hpp.corbaserver import ProblemSolver
Florent Lamiraux's avatar
Florent Lamiraux committed
6
from hpp.corbaserver.wholebody_step.client import Client as WsClient
7
from hpp.corbaserver.wholebody_step import Problem
Florent Lamiraux's avatar
Florent Lamiraux committed
8
9
10
11

Robot.urdfSuffix = '_capsule'
Robot.srdfSuffix= '_capsule'

12
robot = Robot ('hrp2_14')
13
robot.setJointBounds ("base_joint_xyz", [-3, 3, -3, 3, 0, 1])
14
15
ps = ProblemSolver (robot)

Florent Lamiraux's avatar
Florent Lamiraux committed
16
17
cl = robot.client

18
r = Viewer (ps)
Florent Lamiraux's avatar
Florent Lamiraux committed
19
20
21
22
23
q0 = robot.getInitialConfig ()
r (q0)

# Add constraints
wcl = WsClient ()
24
wcl.problem.addStaticStabilityConstraints ("balance", q0, robot.leftAnkle,
25
26
                                           robot.rightAnkle, "",
                                           Problem.SLIDING)
27

Florent Lamiraux's avatar
Florent Lamiraux committed
28
ps.setNumericalConstraints ("balance", ["balance/relative-com",
29
30
31
32
33
                                                "balance/relative-orientation",
                                                "balance/relative-position",
                                                "balance/orientation-left-foot",
                                                "balance/position-left-foot"])

Florent Lamiraux's avatar
Florent Lamiraux committed
34
35
# lock hands in closed position
lockedDofs = robot.leftHandClosed ()
36
for name, value in lockedDofs.iteritems ():
37
    ps.lockJoint (name, value)
Florent Lamiraux's avatar
Florent Lamiraux committed
38
39

lockedDofs = robot.rightHandClosed ()
40
for name, value in lockedDofs.iteritems ():
41
    ps.lockJoint (name, value)
Florent Lamiraux's avatar
Florent Lamiraux committed
42
43
44
45


q1 = [0.0, 0.0, 0.705, 1.0, 0., 0., 0.0, 0.0, 0.0, 0.0, 0.0, -0.4, 0, -1.2, -1.0, 0.0, 0.0, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.261799, -0.17453, 0.0, -0.523599, 0.0, 0.0, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0]

Florent Lamiraux's avatar
Florent Lamiraux committed
46
res = ps.applyConstraints (q1)
47
48
49
50
51
if res [0]:
    q1proj = res [1]
else:
    raise RuntimeError ("Failed to apply constraint.")

Florent Lamiraux's avatar
Florent Lamiraux committed
52
53
54

q2 = [0.0, 0.0, 0.705, 1, 0, 0, 0, 0.0, 0.0, 0.0, 0.0, 1.0, 0, -1.4, -1.0, 0.0, 0.0, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.261799, -0.17453, 0.0, -0.523599, 0.0, 0.0, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0]

Florent Lamiraux's avatar
Florent Lamiraux committed
55
res = ps.applyConstraints (q2)
56
57
58
59
if res [0]:
    q2proj = res [1]
else:
    raise RuntimeError ("Failed to apply constraint.")
Florent Lamiraux's avatar
Florent Lamiraux committed
60

Florent Lamiraux's avatar
Florent Lamiraux committed
61
62
63
ps.setInitialConfig (q1proj)
ps.addGoalConfig (q2proj)
ps.solve ()
Florent Lamiraux's avatar
Florent Lamiraux committed
64

Florent Lamiraux's avatar
Florent Lamiraux committed
65
66
pp = PathPlayer (cl, r)
pp (1)