class HRP2ContactGenerator(AbstractContactGenerator):
def __init__(self, path_planner):
super().__init__(path_planner)
self.robustness = 1
self.robot_node_name = "hrp2_14"
def load_fullbody(self):
from hpp.corbaserver.rbprm.hrp2 import Robot
self.fullbody = Robot()
self.q_ref = self.fullbody.referenceConfig[::] + [0] * self.path_planner.extra_dof
self.weight_postural = self.fullbody.postureWeights[::] + [0] * self.path_planner.extra_dof
self.fullbody.limbs_names=[self.fullbody.rLegId, self.fullbody.lLegId]
def set_joints_bounds(self):
super().set_joints_bounds()
# increase min bounds on knee, required to stay in the 'comfort zone' of the stabilizer
self.fullbody.setJointBounds("LLEG_JOINT3", [0.4, 2.61799])
self.fullbody.setJointBounds("RLEG_JOINT3", [0.4, 2.61799])
def init_viewer(self):
super().init_viewer()
self.v.addLandmark('hrp2_14/base_link', 0.3)
from hpp.corbaserver.rbprm.hrp2 import Robot
from hpp.gepetto import Viewer
from tools.display_tools import *
import time
print "Plan guide trajectory ..."
import hrp2_flatGround_path as tp
print "Done."
import time
pId = tp.ps.numberPaths() - 1
fullBody = Robot()
# Set the bounds for the root
fullBody.setJointBounds("root_joint", [-5, 5, -1.5, 1.5, 0.5, 0.8])
fullBody.setJointBounds(
"LLEG_JOINT3", [0.4, 2.61799]
) # increase min bounds on knee, required to stay in the 'comfort zone' of the stabilizer
fullBody.setJointBounds("RLEG_JOINT3", [0.4, 2.61799])
# TODO : fix rot y legs
# add the 6 extraDof for velocity and acceleration (see *_path.py script)
fullBody.client.robot.setDimensionExtraConfigSpace(tp.extraDof)
fullBody.client.robot.setExtraConfigSpaceBounds([
-tp.vMax, tp.vMax, -tp.vMax, tp.vMax, 0, 0, -tp.aMax, tp.aMax, -tp.aMax,
tp.aMax, 0, 0
])
ps = tp.ProblemSolver(fullBody)
ps.setParameter("Kinodynamic/velocityBound", tp.vMax)
ps.setParameter("Kinodynamic/accelerationBound", tp.aMax)
#load the viewer
from hpp.corbaserver.rbprm.hrp2 import Robot
from hpp.gepetto import Viewer
from tools.display_tools import *
import time
print "Plan guide trajectory ..."
import hrp2_plateformes_path as tp
print "Done."
import time
pId = tp.ps.numberPaths() - 1
fullBody = Robot()
# Set the bounds for the root
fullBody.setJointBounds("root_joint", [-5, 5, -1.5, 1.5, 0.65, 0.9])
#fullBody.setConstrainedJointsBounds()
#fullBody.setJointBounds('leg_left_1_joint',[-0.1,0.2])
#fullBody.setJointBounds('leg_right_1_joint',[-0.2,0.1])
# add the 6 extraDof for velocity and acceleration (see *_path.py script)
fullBody.client.robot.setDimensionExtraConfigSpace(tp.extraDof)
fullBody.client.robot.setExtraConfigSpaceBounds([
-tp.vMax, tp.vMax, -tp.vMax, tp.vMax, 0, 0, -tp.aMax, tp.aMax, -tp.aMax,
tp.aMax, 0, 0
])
ps = tp.ProblemSolver(fullBody)
ps.setParameter("Kinodynamic/velocityBound", tp.vMax)
ps.setParameter("Kinodynamic/accelerationBound", tp.aMax)
#load the viewer
v = tp.Viewer(ps, viewerClient=tp.v.client, displayCoM=True)
# load a reference configuration
q_ref = fullBody.referenceConfig[::] + [0] * 6
from hpp.corbaserver.rbprm.hrp2 import Robot
from hpp.gepetto import Viewer
import stair_bauzil_hrp2_path as tp
import time
fullBody = Robot()
fullBody.setJointBounds("root_joint", [-0.135, 2, -1, 1, 0, 2.2])
ps = tp.ProblemSolver(fullBody)
r = tp.Viewer(ps, viewerClient=tp.r.client)
cType = "_6_DOF"
#~ AFTER loading obstacles
fullBody.addLimb(fullBody.rLegId, fullBody.rLeg, '', fullBody.rLegOffset,
fullBody.rLegNormal, fullBody.rLegx, fullBody.rLegy, 10000,
"manipulability", 0.2, cType)
fullBody.addLimb(fullBody.lLegId, fullBody.lLeg, '', fullBody.lLegOffset,
fullBody.rLegNormal, fullBody.lLegx, fullBody.lLegy, 10000,
"manipulability", 0.2, cType)
fullBody.addLimb(fullBody.rarmId, fullBody.rarm, fullBody.rHand,
fullBody.rArmOffset, fullBody.rArmNormal, fullBody.rArmx,
fullBody.rArmy, 10000, "manipulability", 0.1, "_6_DOF", True)
#~ fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, "manipulability", 0.05, "_6_DOF", True)
rKneeId = '0RKnee'
rLeg = 'RLEG_JOINT0'
from hpp.corbaserver.rbprm.hrp2 import Robot
from hpp.gepetto import Viewer
from tools.display_tools import *
import time
print("Plan guide trajectory ...")
from . import lp_stair_bauzil_hrp2_path_10 as tp
print("Done.")
import time
pId = tp.ps.numberPaths() -1
fullBody = Robot ()
# Set the bounds for the root
fullBody.setJointBounds ("root_joint", [-10.135,2, -20, 20, 0, 2.8])
#fullBody.setConstrainedJointsBounds()
#fullBody.setJointBounds('leg_left_1_joint',[-0.1,0.2])
#fullBody.setJointBounds('leg_right_1_joint',[-0.2,0.1])
# add the 6 extraDof for velocity and acceleration (see *_path.py script)
fullBody.client.robot.setDimensionExtraConfigSpace(0)
#~ fullBody.client.robot.setExtraConfigSpaceBounds([-tp.vMax,tp.vMax,-tp.vMax,tp.vMax,0,0,-tp.aMax,tp.aMax,-tp.aMax,tp.aMax,0,0])
ps = tp.ProblemSolver( fullBody )
#~ ps.setParameter("Kinodynamic/velocityBound",tp.vMax)
#~ ps.setParameter("Kinodynamic/accelerationBound",tp.aMax)
#load the viewer
v = tp.Viewer (ps,viewerClient=tp.r.client, displayCoM = True)
# load a reference configuration
#~ q_ref = fullBody.referenceConfig[::]+[0]*6
q_ref = fullBody.referenceConfig[::]
q_init = q_ref[::]
fullBody.setReferenceConfig(q_ref)
def load_fullbody(self):
from hpp.corbaserver.rbprm.hrp2 import Robot
self.fullbody = Robot()
self.q_ref = self.fullbody.referenceConfig[::] + [0] * self.path_planner.extra_dof
self.weight_postural = self.fullbody.postureWeights[::] + [0] * self.path_planner.extra_dof
self.fullbody.limbs_names=[self.fullbody.rLegId, self.fullbody.lLegId]