test_simple_graph.py 3.76 KB
Newer Older
1
# vim: foldmethod=marker foldlevel=2
2
from hpp.corbaserver.manipulation.hrp2 import Robot
3
from hpp.corbaserver.manipulation import ProblemSolver, ConstraintGraph
4
from hpp.gepetto.manipulation import Viewer
5

6
7
8
9
10
11
12
class ScrewGun (object):
  rootJointType = 'freeflyer'
  packageName = 'airbus_environment'
  meshPackageName = 'airbus_environment'
  urdfName = 'screw_gun'
  urdfSuffix = "_massless"
  srdfSuffix = ""
13
14

Robot.urdfSuffix = '_capsule_mesh'
15
Robot.srdfSuffix = '_manipulation'
16

17
# Load HRP2 and a screwgun {{{3
18
robot = Robot ('hrp2-screw', 'hrp2')
19
20
21
ps = ProblemSolver (robot)
ps.selectPathPlanner ("M-RRT")
r = Viewer (ps)
22
23
r.loadObjectModel (ScrewGun, 'screw_gun')
r.buildCompositeRobot (['hrp2', 'screw_gun'])
24
for d in ["hrp2", "screw_gun"]:
25
  robot.setJointBounds (d+"/base_joint_xyz", [-4,4,-4,4,-4,4])
26

27
28
29
ps.selectPathOptimizer ('None')
ps.setErrorThreshold (1e-3)
ps.setMaxIterations (40)
30
# 3}}}
31

32
# Define configurations {{{3
33
34
35
36
37
38
39
40
41
42
half_sitting = q = robot.getInitialConfig ()
q_init = half_sitting [::]
# Set initial position of screw-driver
q_init [-7:] = [2, 1, 0.65, 0.7071067811865476, 0, -0.7071067811865475, 0]
# Open left hand
ilh = robot.rankInConfiguration ['hrp2/LARM_JOINT6']
q_init [ilh:ilh+6] = [0.75, -0.75, 0.75, -0.75, 0.75, -0.75]

q_goal = q_init [::]
q_goal [-7:] = [2, -1, 0.65, 0.7071067811865476, 0, -0.7071067811865475, 0]
43

44
# 3}}}
45

46
47
# Generate constraints {{{3
graph = ConstraintGraph (robot, 'graph')
48
49
50
51
52
53
54
55
56
57
58
59


jointNames = dict ()
jointNames['all'] = robot.getJointNames ()
jointNames['hrp2'] = list ()
jointNames['allButHRP2LeftArm'] = list ()
for n in jointNames['all']:
  if n.startswith ("hrp2"):
    jointNames['hrp2'].append (n)
  if not n.startswith ("hrp2/LARM"):
    jointNames['allButHRP2LeftArm'].append (n)

60
61
graph.createGrasp ('l_grasp', 'hrp2/leftHand', 'screw_gun/handle2', jointNames ['hrp2'])
graph.createPreGrasp ('l_pregrasp', 'hrp2/leftHand', 'screw_gun/handle2')
62

Joseph Mirabel's avatar
Joseph Mirabel committed
63
64
lockscrewgun = ps.lockFreeFlyerJoint ('screw_gun/base_joint', 'screwgun_lock',
    compType = 'Equality')
65

66
locklhand = ['larm_6','lhand_0','lhand_1','lhand_2','lhand_3','lhand_4']
Joseph Mirabel's avatar
Joseph Mirabel committed
67
68
ps.createLockedJoint ('larm_6' , 'hrp2/LARM_JOINT6' , [q_init[ilh],])
ps.createLockedJoint \
Florent Lamiraux's avatar
Florent Lamiraux committed
69
    ('lhand_0', 'hrp2/LHAND_JOINT0', [q_init[ilh + 1],])
Joseph Mirabel's avatar
Joseph Mirabel committed
70
ps.createLockedJoint \
Florent Lamiraux's avatar
Florent Lamiraux committed
71
    ('lhand_1', 'hrp2/LHAND_JOINT1', [q_init[ilh + 2],])
Joseph Mirabel's avatar
Joseph Mirabel committed
72
ps.createLockedJoint \
Florent Lamiraux's avatar
Florent Lamiraux committed
73
    ('lhand_2', 'hrp2/LHAND_JOINT2', [q_init[ilh + 3],])
Joseph Mirabel's avatar
Joseph Mirabel committed
74
ps.createLockedJoint \
Florent Lamiraux's avatar
Florent Lamiraux committed
75
    ('lhand_3', 'hrp2/LHAND_JOINT3', [q_init[ilh + 4],])
Joseph Mirabel's avatar
Joseph Mirabel committed
76
ps.createLockedJoint \
Florent Lamiraux's avatar
Florent Lamiraux committed
77
    ('lhand_4', 'hrp2/LHAND_JOINT4', [q_init[ilh + 5],])
78
79

ps.createStaticStabilityConstraints ("balance", q_init)
80
81
82
83
# 3}}}

# Create the graph of constraints {{{3
graph.createNode (["screwgun", "free"])
84

85
graph.createWaypointEdge ('screwgun', 'free', 'ungrasp', nb=1, weight=1)
86

87
graph.createWaypointEdge ('free', 'screwgun', 'grasp', nb=1, weight=5)
88

89
graph.createEdge ('free', 'free', 'move_free', 5)
90

91
92
93
94
95
96
97
98
99
100
101
102
graph.createEdge ('screwgun', 'screwgun', 'keep_grasp', 10)
graph.createLevelSetEdge ('screwgun', 'screwgun', 'keep_grasp_ls', 5)

graph.setConstraints (node='screwgun', grasp='l_grasp')
graph.setConstraints (edge='move_free', lockDof = lockscrewgun)
graph.setConstraints (edge='ungrasp_e0', lockDof = lockscrewgun)
graph.setConstraints (node='ungrasp_n0', pregrasp = 'l_pregrasp')
graph.setConstraints (edge='ungrasp_e1', lockDof = lockscrewgun)
graph.setConstraints (edge='grasp_e0', lockDof = lockscrewgun)
graph.setConstraints (node='grasp_n0', pregrasp = 'l_pregrasp')
graph.setConstraints (edge='grasp_e1', lockDof = lockscrewgun)
graph.client.graph.setLevelSetConstraints (graph.edges['keep_grasp_ls'], [], lockscrewgun)
103
graph.setConstraints (graph=True, lockDof = locklhand, numConstraints=ps.balanceConstraints ())
104
# 3}}}
105

106
107
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
108
109
110

from hpp.gepetto import PathPlayer
pp = PathPlayer (robot.client.basic, r)