from hpp.corbaserver.rbprm.anymal_abstract import Robot
from hpp.gepetto import Viewer
from hpp.corbaserver import ProblemSolver
import numpy as np
import time
#~ statusFilename = "/res/infos.log"
statusFilename = "/tmp/infos.log"
vMax = 0.3 # linear velocity bound for the root
aMax = 1. # linear acceleration bound for the root
extraDof = 6
mu = 0.5 # coefficient of friction
# Creating an instance of the helper class, and loading the robot
# Creating an instance of the helper class, and loading the robot
rbprmBuilder = Robot()
# Define bounds for the root : bounding box of the scenario
root_bounds = [-2, 2, -2, 2, 0.4, 0.5]
rbprmBuilder.setJointBounds("root_joint", root_bounds)
# The following lines set constraint on the valid configurations:
# a configuration is valid only if all limbs can create a contact with the corresponding afforcances type
rbprmBuilder.setFilter(rbprmBuilder.urdfNameRom)
for rom in rbprmBuilder.urdfNameRom:
rbprmBuilder.setAffordanceFilter(rom, ['Support'])
# We also bound the rotations of the torso. (z, y, x)
rbprmBuilder.boundSO3([-1.7, 1.7, -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)
rbprmBuilder.client.robot.setExtraConfigSpaceBounds(
def load_rbprm(self):
from hpp.corbaserver.rbprm.anymal_abstract import Robot
self.rbprmBuilder = Robot()
import math
statusFilename = "infos.log"
REF_Z_VALUE = 0.47
Z_VALUE_LOGS = REF_Z_VALUE + 0.13
Z_VALUE_PALET = REF_Z_VALUE + 0.165
Z_VALUE_WOOD = REF_Z_VALUE + 0.15
Y_VALUE = -0.13
vInit = 0.01
vMax = 0.5# linear velocity bound for the root
aMax = 0.5# linear acceleration bound for the root
aMaxZ = 5.
extraDof = 6
mu=0.5# coefficient of friction
# Creating an instance of the helper class, and loading the robot
# Creating an instance of the helper class, and loading the robot
rbprmBuilder = Robot ()
# Define bounds for the root : bounding box of the scenario
rootBounds = [-1.201,1.11, Y_VALUE-0.001, Y_VALUE+0.001, Z_VALUE_LOGS - 0.001, Z_VALUE_PALET + 0.06]
rbprmBuilder.setJointBounds ("root_joint", rootBounds)
# The following lines set constraint on the valid configurations:
# a configuration is valid only if all limbs can create a contact with the corresponding afforcances type
rbprmBuilder.setFilter(Robot.urdfNameRom)
for rom in rbprmBuilder.urdfNameRom :
rbprmBuilder.setAffordanceFilter(rom, ['Support'])
# We also bound the rotations of the torso. (z, y, x)
rbprmBuilder.boundSO3([-0.01,0.01,-0.01,0.01,-0.01,0.01])
# 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)
from hpp.corbaserver.rbprm.anymal_abstract import Robot
from hpp.gepetto import Viewer
from hpp.corbaserver import ProblemSolver
import numpy as np
import time
statusFilename = "/tmp/infos.log"
vMax = 0.3# linear velocity bound for the root
aMax = 1.# linear acceleration bound for the root
extraDof = 6
mu=0.5# coefficient of friction
# Creating an instance of the helper class, and loading the robot
# Creating an instance of the helper class, and loading the robot
rbprmBuilder = Robot ()
# Define bounds for the root : bounding box of the scenario
root_bounds = cfg.ROOT_BOUNDS
rbprmBuilder.setJointBounds ("root_joint", root_bounds)
# The following lines set constraint on the valid configurations:
# a configuration is valid only if all limbs can create a contact with the corresponding afforcances type
rbprmBuilder.setFilter(rbprmBuilder.urdfNameRom)
for rom in rbprmBuilder.urdfNameRom :
rbprmBuilder.setAffordanceFilter(rom, ['Support'])
#~ rbprmBuilder.setAffordanceFilter("front_prong", ['Support'])
# We also bound the rotations of the torso. (z, y, x)
#~ rbprmBuilder.boundSO3([-1.7,1.7,-0.1,0.1,-0.1,0.1])
rbprmBuilder.boundSO3([-3,3,-3.,3.,-3.,-3])
# Add 6 extraDOF to the problem, used to store the linear velocity and acceleration of the root