rbprmBuilder.setJointBounds("root_joint", [0, 2, -1, 1, 0, 2.2])
#~ rbprmBuilder.setFilter(['hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom'])
rbprmBuilder.setAffordanceFilter('3Rarm', ['Support'])
rbprmBuilder.setAffordanceFilter('0rLeg', [
'Support',
])
rbprmBuilder.setAffordanceFilter('1lLeg', ['Support'])
#~ rbprmBuilder.setNormalFilter('hrp2_rarm_rom', [0,0,1], 0.5)
#~ rbprmBuilder.setNormalFilter('hrp2_lleg_rom', [0,0,1], 0.9)
#~ rbprmBuilder.setNormalFilter('hrp2_rleg_rom', [0,0,1], 0.9)
#~ rbprmBuilder.setNormalFilter('hyq_rhleg_rom', [0,0,1], 0.9)
rbprmBuilder.boundSO3([-0., 0, -1, 1, -1, 1])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from hpp.corbaserver.problem_solver import ProblemSolver
ps = ProblemSolver(rbprmBuilder)
r = Viewer(ps)
q_init = rbprmBuilder.getCurrentConfig()
q_init[0:3] = [0, -0.82, 0.648702]
rbprmBuilder.setCurrentConfig(q_init)
r(q_init)
q_init[0:3] = [0.1, -0.82, 0.648702]
rbprmBuilder.setCurrentConfig(q_init)
r(q_init)
#~ q_init [0:3] = [0, -0.63, 0.6]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
#~ q_init [3:7] = [ 0.98877108, 0. , 0.14943813, 0. ]
q_goal = q_init[::]
q_goal[3:7] = [0., 0., 0.14943813, 0.98877108]
q_goal[0:3] = [1.49, -0.65, 1.25]
# Information to retrieve urdf and srdf files.
urdfName = "hyq"
urdfSuffix = ""
srdfSuffix = ""
# This time we load the full body model of HyQ
fullBody = FullBody()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName,
packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds("root_joint", [-2, 5, -1, 1, 0.3, 4])
# Setting a number of sample configurations used
nbSamples = 10000
ps = ProblemSolver(fullBody)
r = Viewer(ps)
q_init = hyq_ref[:]
r(q_init)
from hppy_ik import *
from numpy import array
from numpy.linalg import norm
import eigenpy
eigenpy.switchToNumpyArray()
__EPS = 1e-4
'Support',
])
rbprmBuilder.setAffordanceFilter(Robot.lLegId, ['Support'])
rbprmBuilder.boundSO3([-4., 4., -0.1, 0.1, -0.1, 0.1])
# Add 6 extraDOF to the problem, used to store the linear velocity and acceleration of the root
rbprmBuilder.client.robot.setDimensionExtraConfigSpace(extraDof)
# We set the bounds of this extraDof with velocity and acceleration bounds (expect on z axis)
extraDofBounds = [
-vMax, vMax, -vMax, vMax, -10., 10., -aMax, aMax, -aMax, aMax, -10., 10.
]
rbprmBuilder.client.robot.setExtraConfigSpaceBounds(extraDofBounds)
indexECS = rbprmBuilder.getConfigSize(
) - rbprmBuilder.client.robot.getDimensionExtraConfigSpace()
from hpp.corbaserver.problem_solver import ProblemSolver
ps = ProblemSolver(rbprmBuilder)
from hpp.gepetto import ViewerFactory
vf = ViewerFactory(ps)
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool()
afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
afftool.loadObstacleModel(packageName,
"multicontact/slalom_debris",
"planning",
vf,
reduceSizes=[0., 0., 0.])
v = vf.createViewer(displayArrows=True)
afftool.visualiseAffordances('Support', v, [0.25, 0.5, 0.5])
v.addLandmark(v.sceneName, 1)
#~ urdfSuffix = ""
#~ srdfSuffix = ""
rbprmBuilder = Robot()
rbprmBuilder.setJointBounds("root_joint", [-2, 2, -1, 1, 0, 2.2])
rbprmBuilder.setFilter([Robot.rLegId, Robot.lLegId])
rbprmBuilder.setAffordanceFilter(Robot.rLegId, [
'Support',
])
rbprmBuilder.setAffordanceFilter(Robot.lLegId, ['Support'])
rbprmBuilder.boundSO3([-0., 0, -1, 1, -1, 1])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from hpp.corbaserver.problem_solver import ProblemSolver
ps = ProblemSolver(rbprmBuilder)
r = Viewer(ps)
q_init = rbprmBuilder.getCurrentConfig()
q_init[0:3] = [-1, -0.82, 0.5]
rbprmBuilder.setCurrentConfig(q_init)
r(q_init)
q_goal = q_init[::]
q_goal[3:7] = [0., 0., 0.14943813, 0.98877108]
q_goal[0:3] = [1.49, -0.65, 1.2]
r(q_goal)
ps.addPathOptimizer("RandomShortcut")
#~ ps.setInitialConfig (q_init)
#~ ps.addGoalConfig (q_goal)
# Import Gepetto viewer helpwer class
from hpp.gepetto import Viewer
# Import Problem solver (holds most of the generated data)
from hpp.corbaserver.problem_solver import ProblemSolver
# Import robot. Needed to create a robot instance for the viewer application
from hpp.corbaserver.affordance import Robot
# Create instance of the hyq robot, the problem solver and the viewer
robot = Robot('hyq')
ps = ProblemSolver(robot)
r = Viewer(ps)
# Setting initial configuration to show on the viewer
q_init = robot.getCurrentConfig()
q_init[0:3] = [-2, 0, 0.63]
robot.setCurrentConfig(q_init)
r(q_init)
# Set coulour variables for different affordance types
SupportColour = [0.0, 0.95, 0.80]
LeanColour = [0.9, 0.5, 0]
# Import the affordance helper class to extract useful surface
# objects from the environment, and create an instance of affordanceTool
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool()
# Set the affordance configuration. The configuration vector has size 3
# and comprises the error margin, the angle margin for neighbouring triangles
# and the minimum area, in that order.
# If no configuration is set, a default configuration is used.
# Import Gepetto viewer helpwer class
from hpp.gepetto import Viewer
# Import Problem solver (holds most of the generated data)
from hpp.corbaserver.problem_solver import ProblemSolver
# Import robot. Needed to create a robot instance for the viewer application
from hpp.corbaserver.affordance import Robot
# Create instance of the hyq robot, the problem solver and the viewer
robot = Robot('hyq')
ps = ProblemSolver(robot)
r = Viewer(ps)
# Setting initial configuration to show on the viewer
q_init = robot.getCurrentConfig()
q_init[0:3] = [-2, 0, 0.63]
robot.setCurrentConfig(q_init)
r(q_init)
# Set coulour variables for different affordance types
SupportColour = [0.0, 0.95, 0.80]
LeanColour = [0.9, 0.5, 0]
# Import the affordance helper class to extract useful surface
# objects from the environment, and create an instance of affordanceTool
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool()
afftool.setAffordanceConfig('Support', [0.3, 0.3, 0.05])
afftool.setAffordanceConfig('Lean', [0.1, 0.3, 0.05])
# Load obstacle models and visualise affordances. When loading an obstacle,
extraDof = 6
mu=0.5# coefficient of friction
rbprmBuilder.setFilter([Robot.rLegId,Robot.lLegId])
rbprmBuilder.setAffordanceFilter(Robot.rLegId, ['Support',])
rbprmBuilder.setAffordanceFilter(Robot.lLegId, ['Support'])
rbprmBuilder.boundSO3([-4.,4.,-0.1,0.1,-0.1,0.1])
# Add 6 extraDOF to the problem, used to store the linear velocity and acceleration of the root
rbprmBuilder.client.robot.setDimensionExtraConfigSpace(extraDof)
# We set the bounds of this extraDof with velocity and acceleration bounds (expect on z axis)
extraDofBounds = [-vMax,vMax,-vMax,vMax,-10.,10.,-aMax,aMax,-aMax,aMax,-10.,10.]
rbprmBuilder.client.robot.setExtraConfigSpaceBounds(extraDofBounds)
indexECS = rbprmBuilder.getConfigSize() - rbprmBuilder.client.robot.getDimensionExtraConfigSpace()
from hpp.corbaserver.problem_solver import ProblemSolver
ps = ProblemSolver( rbprmBuilder )
from hpp.gepetto import ViewerFactory
vf = ViewerFactory (ps)
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool ()
afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
afftool.loadObstacleModel (packageName, "multicontact/bauzil_ramp_simplified", "planning", vf)#,reduceSizes=[0.05,0.,0.])
v = vf.createViewer(displayArrows = True)
afftool.visualiseAffordances('Support', v, [0.25, 0.5, 0.5])
v.addLandmark(v.sceneName,1)
ps.setParameter("Kinodynamic/velocityBound",vMax)
ps.setParameter("Kinodynamic/accelerationBound",aMax)
# force the orientation of the trunk to match the direction of the motion