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'
meshPackageName = "hrp2_14_description"
rootJointType = "freeflyer"
##
# Information to retrieve urdf and srdf files.
urdfName = "hrp2_14"
urdfSuffix = "_reduced"
srdfSuffix = ""
fullBody = FullBody()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName,
packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds("base_joint_xyz", [-0.135, 2, -1, 1, 0, 2.2])
ps = tp.ProblemSolver(fullBody)
r = tp.Viewer(ps)
#~ AFTER loading obstacles
rLegId = '0rLeg'
rLeg = 'RLEG_JOINT0'
rLegOffset = [
0,
-0.105,
0,
]
rLegNormal = [0, 1, 0]
rLegx = 0.09
rLegy = 0.05
fullBody.addLimb(rLegId, rLeg, '', rLegOffset, rLegNormal, rLegx, rLegy, 10000,
"manipulability", 0.1)
srdfSuffix = ""
fullBody = FullBody()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName,
packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds("base_joint_xyz", [0, 2, -1, 1, 0, 2.2])
fullBody.client.basic.robot.setDimensionExtraConfigSpace(tp.extraDof)
ps = tp.ProblemSolver(fullBody)
ps.client.problem.setParameter("aMax", omniORB.any.to_any(tp.aMax))
ps.client.problem.setParameter("vMax", omniORB.any.to_any(tp.vMax))
ps.client.problem.setParameter("friction", tp.mu)
r = tp.Viewer(ps, viewerClient=tp.r.client, displayCoM=True)
tStart = time.time()
#~ AFTER loading obstacles
#~ AFTER loading obstacles
rLegId = 'hrp2_rleg_rom'
lLegId = 'hrp2_lleg_rom'
tStart = time.time()
rLeg = 'RLEG_JOINT0'
rLegOffset = [0, 0, -0.0955]
rLegLimbOffset = [0, 0, -0.035] #0.035
rLegNormal = [0, 0, 1]
rLegx = 0.09
rLegy = 0.05