Commit 64d37400 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Update scripts.

parent bca2f759
...@@ -6,12 +6,15 @@ from hpp.corbaserver import Client, ProblemSolver ...@@ -6,12 +6,15 @@ from hpp.corbaserver import Client, ProblemSolver
from hpp.corbaserver.wholebody_step import Client as WSClient from hpp.corbaserver.wholebody_step import Client as WSClient
from hpp.corbaserver.wholebody_step import Problem from hpp.corbaserver.wholebody_step import Problem
from hpp.corbaserver.hrp2 import Robot from hpp.corbaserver.hrp2 import Robot
from hpp.gepetto import ViewerFactory
Robot.urdfSuffix = '_capsule' Robot.urdfSuffix = '_capsule'
Robot.srdfSuffix = '_capsule' Robot.srdfSuffix = '_capsule'
robot = Robot ('hrp2') robot = Robot ('hrp2')
p = ProblemSolver (robot) p = ProblemSolver (robot)
vf = ViewerFactory (p)
wcl = WSClient () wcl = WSClient ()
q0 = robot.getInitialConfig () q0 = robot.getInitialConfig ()
...@@ -24,6 +27,14 @@ balanceConstraints = [constraintName + "/relative-com", ...@@ -24,6 +27,14 @@ balanceConstraints = [constraintName + "/relative-com",
constraintName + "/relative-position", constraintName + "/relative-position",
constraintName + "/orientation-left-foot", constraintName + "/orientation-left-foot",
constraintName + "/position-left-foot"] constraintName + "/position-left-foot"]
c2name = "stability"
wcl.problem.addStabilityConstraints (c2name, q0, robot.leftAnkle,
robot.rightAnkle, "", Problem.ALIGNED_COM)
b2C = [ c2name + "/com-between-feet",
c2name + "/orientation-right",
c2name + "/orientation-left",
c2name + "/position-right",
c2name + "/position-left"]
robot.setJointBounds ("base_joint_xyz", [-4,4,-4,4,-4,4]) robot.setJointBounds ("base_joint_xyz", [-4,4,-4,4,-4,4])
...@@ -41,3 +52,8 @@ for constraint in balanceConstraints: ...@@ -41,3 +52,8 @@ for constraint in balanceConstraints:
testConstraint ([constraint]) testConstraint ([constraint])
testConstraint (balanceConstraints) testConstraint (balanceConstraints)
for constraint in b2C:
testConstraint ([constraint])
testConstraint (b2C)
...@@ -5,18 +5,17 @@ from hpp.gepetto.manipulation import Viewer, ViewerFactory ...@@ -5,18 +5,17 @@ from hpp.gepetto.manipulation import Viewer, ViewerFactory
from hpp.gepetto import PathPlayer, PathPlayerGui from hpp.gepetto import PathPlayer, PathPlayerGui
Robot.urdfSuffix = '_capsule_mesh' Robot.urdfSuffix = '_capsule_mesh'
Robot.srdfSuffix = '_manipulation_moveit' Robot.srdfSuffix = '_manipulation'
# Load HRP2 {{{3 # Load HRP2 {{{3
robot = Robot ('hrp2-e', 'hrp2') robot = Robot ('hrp2', 'hrp2')
ps = ProblemSolver (robot) ps = ProblemSolver (robot)
vf = ViewerFactory (ps) vf = ViewerFactory (ps)
vf.buildCompositeRobot (['hrp2'])
robot.setJointBounds ("hrp2/base_joint_xyz", [-0.2,0.8,-0.5,0.5, 0,2]) robot.setJointBounds ("hrp2/base_joint_xyz", [-0.2,0.8,-0.5,0.5, 0,2])
ps.selectPathOptimizer ('None') ps.selectPathOptimizer ('None')
ps.selectPathProjector ('Progressive', 0.02) ps.selectPathProjector ('Progressive', 0.2)
ps.setErrorThreshold (1e-3) ps.setErrorThreshold (1e-3)
ps.setMaxIterations (40) ps.setMaxIterations (40)
# 3}}} # 3}}}
...@@ -32,8 +31,8 @@ q_init [irh:irh+6] = [0.75, -0.75, 0.75, -0.75, 0.75, -0.75] ...@@ -32,8 +31,8 @@ q_init [irh:irh+6] = [0.75, -0.75, 0.75, -0.75, 0.75, -0.75]
ibjxyz = robot.rankInConfiguration ['hrp2/base_joint_xyz'] ibjxyz = robot.rankInConfiguration ['hrp2/base_joint_xyz']
q_goal = q_init [::] q_goal = q_init [::]
# q_goal [ibjxyz:ibjxyz+2] = [0.5, 0] q_goal [ibjxyz:ibjxyz+2] = [0.5, 0]
q_goal [ibjxyz:ibjxyz+2] = [0.2, 0] # q_goal [ibjxyz:ibjxyz+2] = [0.2, 0]
# 3}}} # 3}}}
...@@ -126,7 +125,7 @@ _["right_to_rightside_ls"]="" ...@@ -126,7 +125,7 @@ _["right_to_rightside_ls"]=""
_["rightside_to_double"]="" _["rightside_to_double"]=""
_["right_support"]="" _["right_support"]=""
# 4}}} # 4}}}
graph.setTextToTeXTranslation (_) # graph.setTextToTeXTranslation (_)
graph.createNode (["both_left","both_right","both","left","right"]) graph.createNode (["both_left","both_right","both","left","right"])
......
...@@ -119,6 +119,27 @@ lockAll = lockhands + lockHeadAndTorso ...@@ -119,6 +119,27 @@ lockAll = lockhands + lockHeadAndTorso
# Create the graph. {{{3 # Create the graph. {{{3
_ = dict ()
# Create a dictionnary to translate human readable names into LaTeX expressions.
# This goes well with option -tmath of dot2tex command line.
# {{{4
_["both"]="2 grasps"
_["left"]="Left grasp"
_["right"]="Right grasp"
_["free"]="No grasp"
# _["r_grasp"]=""
# _["l_grasp"]=""
# _["b_r_grasp"]=""
# _["b_l_grasp"]=""
# _["b_r_ungrasp"]=""
# _["b_l_ungrasp"]=""
_["move_free"]=""
_["r_keep_grasp"]=""
_["l_keep_grasp"]=""
# 4}}}
cg.setTextToTeXTranslation (_)
cg.createNode (['both', 'right', 'left', 'free']) cg.createNode (['both', 'right', 'left', 'free'])
cg.setConstraints (node='free', numConstraints=['box_placement']) cg.setConstraints (node='free', numConstraints=['box_placement'])
......
# vim: foldmethod=marker foldlevel=2 # vim: foldmethod=marker foldlevel=2
from hpp.corbaserver.manipulation.hrp2 import Robot from hpp.corbaserver.manipulation.hrp2 import Robot
from hpp.corbaserver.manipulation import ProblemSolver, ConstraintGraph from hpp.corbaserver.manipulation import ProblemSolver, ConstraintGraph
from hpp.gepetto.manipulation import Viewer from hpp.gepetto.manipulation import Viewer, ViewerFactory
from hpp.gepetto import PathPlayer, PathPlayerGui
class ScrewGun (object): class ScrewGun (object):
rootJointType = 'freeflyer' rootJointType = 'freeflyer'
packageName = 'airbus_environment' packageName = 'airbus_environment'
meshPackageName = 'airbus_environment' meshPackageName = 'airbus_environment'
urdfName = 'screw_gun' urdfName = 'screw_gun'
urdfSuffix = "_massless" urdfSuffix = ""
srdfSuffix = "" srdfSuffix = ""
Robot.urdfSuffix = '_capsule_mesh' Robot.urdfSuffix = '_capsule_mesh'
...@@ -18,13 +19,13 @@ Robot.srdfSuffix = '_manipulation' ...@@ -18,13 +19,13 @@ Robot.srdfSuffix = '_manipulation'
robot = Robot ('hrp2-screw', 'hrp2') robot = Robot ('hrp2-screw', 'hrp2')
ps = ProblemSolver (robot) ps = ProblemSolver (robot)
ps.selectPathPlanner ("M-RRT") ps.selectPathPlanner ("M-RRT")
r = Viewer (ps) vf = ViewerFactory (ps)
r.loadObjectModel (ScrewGun, 'screw_gun') vf.loadObjectModel (ScrewGun, 'screw_gun')
r.buildCompositeRobot (['hrp2', 'screw_gun'])
for d in ["hrp2", "screw_gun"]: for d in ["hrp2", "screw_gun"]:
robot.setJointBounds (d+"/base_joint_xyz", [-4,4,-4,4,-4,4]) robot.setJointBounds (d+"/base_joint_xyz", [-4,4,-4,4,-4,4])
ps.selectPathOptimizer ('None') ps.selectPathOptimizer ('None')
ps.selectPathProjector ("Progressive", 0.2)
ps.setErrorThreshold (1e-3) ps.setErrorThreshold (1e-3)
ps.setMaxIterations (40) ps.setMaxIterations (40)
# 3}}} # 3}}}
...@@ -33,7 +34,8 @@ ps.setMaxIterations (40) ...@@ -33,7 +34,8 @@ ps.setMaxIterations (40)
half_sitting = q = robot.getInitialConfig () half_sitting = q = robot.getInitialConfig ()
q_init = half_sitting [::] q_init = half_sitting [::]
# Set initial position of screw-driver # Set initial position of screw-driver
q_init [-7:] = [2, 1, 0.65, 0.7071067811865476, 0, -0.7071067811865475, 0] # q_init [-7:] = [2, 1, 0.65, 0.7071067811865476, 0, -0.7071067811865475, 0]
q_init [-7:] = [2, 1, 0.65, 0.7071067811865476, 0.5, -0.5, 0]
# Open left hand # Open left hand
ilh = robot.rankInConfiguration ['hrp2/LARM_JOINT6'] ilh = robot.rankInConfiguration ['hrp2/LARM_JOINT6']
q_init [ilh:ilh+6] = [0.75, -0.75, 0.75, -0.75, 0.75, -0.75] q_init [ilh:ilh+6] = [0.75, -0.75, 0.75, -0.75, 0.75, -0.75]
...@@ -57,7 +59,8 @@ for n in jointNames['all']: ...@@ -57,7 +59,8 @@ for n in jointNames['all']:
if not n.startswith ("hrp2/LARM"): if not n.startswith ("hrp2/LARM"):
jointNames['allButHRP2LeftArm'].append (n) jointNames['allButHRP2LeftArm'].append (n)
graph.createGrasp ('l_grasp', 'hrp2/leftHand', 'screw_gun/handle2', jointNames ['hrp2']) ps.addPassiveDofs ('hrp2', jointNames ['hrp2'])
graph.createGrasp ('l_grasp', 'hrp2/leftHand', 'screw_gun/handle2', 'hrp2')
graph.createPreGrasp ('l_pregrasp', 'hrp2/leftHand', 'screw_gun/handle2') graph.createPreGrasp ('l_pregrasp', 'hrp2/leftHand', 'screw_gun/handle2')
lockscrewgun = ps.lockFreeFlyerJoint ('screw_gun/base_joint', 'screwgun_lock', lockscrewgun = ps.lockFreeFlyerJoint ('screw_gun/base_joint', 'screwgun_lock',
...@@ -76,7 +79,8 @@ ps.createLockedJoint \ ...@@ -76,7 +79,8 @@ ps.createLockedJoint \
ps.createLockedJoint \ ps.createLockedJoint \
('lhand_4', 'hrp2/LHAND_JOINT4', [q_init[ilh + 5],]) ('lhand_4', 'hrp2/LHAND_JOINT4', [q_init[ilh + 5],])
ps.createStaticStabilityConstraints ("balance", q_init) ps.addPartialCom ('hrp2', ['hrp2/base_joint_xyz'])
ps.createStaticStabilityConstraints ("balance-hrp2", q_init, 'hrp2')
# 3}}} # 3}}}
# Create the graph of constraints {{{3 # Create the graph of constraints {{{3
...@@ -99,12 +103,9 @@ graph.setConstraints (edge='ungrasp_e1', lockDof = lockscrewgun) ...@@ -99,12 +103,9 @@ graph.setConstraints (edge='ungrasp_e1', lockDof = lockscrewgun)
graph.setConstraints (edge='grasp_e0', lockDof = lockscrewgun) graph.setConstraints (edge='grasp_e0', lockDof = lockscrewgun)
graph.setConstraints (node='grasp_n0', pregrasp = 'l_pregrasp') graph.setConstraints (node='grasp_n0', pregrasp = 'l_pregrasp')
graph.setConstraints (edge='grasp_e1', lockDof = lockscrewgun) graph.setConstraints (edge='grasp_e1', lockDof = lockscrewgun)
graph.client.graph.setLevelSetConstraints (graph.edges['keep_grasp_ls'], [], lockscrewgun) graph.setLevelSetConstraints ('keep_grasp_ls', lockDof = lockscrewgun)
graph.setConstraints (graph=True, lockDof = locklhand, numConstraints=ps.balanceConstraints ()) graph.setConstraints (graph=True, lockDof = locklhand, numConstraints=ps.balanceConstraints ())
# 3}}} # 3}}}
ps.setInitialConfig (q_init) ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal) ps.addGoalConfig (q_goal)
from hpp.gepetto import PathPlayer
pp = PathPlayer (robot.client.basic, r)
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