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
from hpp.corbaserver.wholebody_step import Client as WSClient
from hpp.corbaserver.wholebody_step import Problem
from hpp.corbaserver.hrp2 import Robot
from hpp.gepetto import ViewerFactory
Robot.urdfSuffix = '_capsule'
Robot.srdfSuffix = '_capsule'
robot = Robot ('hrp2')
p = ProblemSolver (robot)
vf = ViewerFactory (p)
wcl = WSClient ()
q0 = robot.getInitialConfig ()
......@@ -24,6 +27,14 @@ balanceConstraints = [constraintName + "/relative-com",
constraintName + "/relative-position",
constraintName + "/orientation-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])
......@@ -41,3 +52,8 @@ for constraint in balanceConstraints:
testConstraint ([constraint])
testConstraint (balanceConstraints)
for constraint in b2C:
testConstraint ([constraint])
testConstraint (b2C)
......@@ -5,18 +5,17 @@ from hpp.gepetto.manipulation import Viewer, ViewerFactory
from hpp.gepetto import PathPlayer, PathPlayerGui
Robot.urdfSuffix = '_capsule_mesh'
Robot.srdfSuffix = '_manipulation_moveit'
Robot.srdfSuffix = '_manipulation'
# Load HRP2 {{{3
robot = Robot ('hrp2-e', 'hrp2')
robot = Robot ('hrp2', 'hrp2')
ps = ProblemSolver (robot)
vf = ViewerFactory (ps)
vf.buildCompositeRobot (['hrp2'])
robot.setJointBounds ("hrp2/base_joint_xyz", [-0.2,0.8,-0.5,0.5, 0,2])
ps.selectPathOptimizer ('None')
ps.selectPathProjector ('Progressive', 0.02)
ps.selectPathProjector ('Progressive', 0.2)
ps.setErrorThreshold (1e-3)
ps.setMaxIterations (40)
# 3}}}
......@@ -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']
q_goal = q_init [::]
# q_goal [ibjxyz:ibjxyz+2] = [0.5, 0]
q_goal [ibjxyz:ibjxyz+2] = [0.2, 0]
q_goal [ibjxyz:ibjxyz+2] = [0.5, 0]
# q_goal [ibjxyz:ibjxyz+2] = [0.2, 0]
# 3}}}
......@@ -126,7 +125,7 @@ _["right_to_rightside_ls"]=""
_["rightside_to_double"]=""
_["right_support"]=""
# 4}}}
graph.setTextToTeXTranslation (_)
# graph.setTextToTeXTranslation (_)
graph.createNode (["both_left","both_right","both","left","right"])
......
......@@ -119,6 +119,27 @@ lockAll = lockhands + lockHeadAndTorso
# 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.setConstraints (node='free', numConstraints=['box_placement'])
......
# vim: foldmethod=marker foldlevel=2
from hpp.corbaserver.manipulation.hrp2 import Robot
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):
rootJointType = 'freeflyer'
packageName = 'airbus_environment'
meshPackageName = 'airbus_environment'
urdfName = 'screw_gun'
urdfSuffix = "_massless"
urdfSuffix = ""
srdfSuffix = ""
Robot.urdfSuffix = '_capsule_mesh'
......@@ -18,13 +19,13 @@ Robot.srdfSuffix = '_manipulation'
robot = Robot ('hrp2-screw', 'hrp2')
ps = ProblemSolver (robot)
ps.selectPathPlanner ("M-RRT")
r = Viewer (ps)
r.loadObjectModel (ScrewGun, 'screw_gun')
r.buildCompositeRobot (['hrp2', 'screw_gun'])
vf = ViewerFactory (ps)
vf.loadObjectModel (ScrewGun, 'screw_gun')
for d in ["hrp2", "screw_gun"]:
robot.setJointBounds (d+"/base_joint_xyz", [-4,4,-4,4,-4,4])
ps.selectPathOptimizer ('None')
ps.selectPathProjector ("Progressive", 0.2)
ps.setErrorThreshold (1e-3)
ps.setMaxIterations (40)
# 3}}}
......@@ -33,7 +34,8 @@ ps.setMaxIterations (40)
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]
# 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
ilh = robot.rankInConfiguration ['hrp2/LARM_JOINT6']
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']:
if not n.startswith ("hrp2/LARM"):
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')
lockscrewgun = ps.lockFreeFlyerJoint ('screw_gun/base_joint', 'screwgun_lock',
......@@ -76,7 +79,8 @@ ps.createLockedJoint \
ps.createLockedJoint \
('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}}}
# Create the graph of constraints {{{3
......@@ -99,12 +103,9 @@ 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)
graph.setLevelSetConstraints ('keep_grasp_ls', lockDof = lockscrewgun)
graph.setConstraints (graph=True, lockDof = locklhand, numConstraints=ps.balanceConstraints ())
# 3}}}
ps.setInitialConfig (q_init)
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