srdfSuffix = ""
pId = tp.ps.numberPaths() - 1
fullBody = FullBody()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName,
packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds("base_joint_xyz", [-1.2, 1.5, -0.05, 0.05, 0.55, 0.85])
fullBody.client.basic.robot.setDimensionExtraConfigSpace(tp.extraDof)
fullBody.client.basic.robot.setExtraConfigSpaceBounds(
[-0, 0, -0, 0, -0, 0, 0, 0, 0, 0, 0, 0])
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))
r = tp.Viewer(ps,
viewerClient=tp.r.client,
displayArrows=True,
displayCoM=True)
q_init = [
0, 0, 0.648702, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.261799388,
0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17, 0.261799388, -0.174532925,
0.0, -0.523598776, 0.0, 0.0, 0.17, 0.0, 0.0, -0.453785606, 0.872664626,
-0.41887902, 0.0, 0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, 0,
0, 0, 0, 0, 0
]
r(q_init)
q_ref = q_init[::]
fullBody.setCurrentConfig(q_init)
fullBody.setReferenceConfig(q_ref)
#~ AFTER loading obstacles
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", [-2, 5, -1, 1, 0.3, 4])
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,
"forward", 0.03)