import HRP4
set_printoptions(precision=5)
################# Loading the environment ########################
env = Environment() # create openrave environment
env.SetViewer('qtcoin') # attach viewer (optional)
env.Load('/home/cuong/Dropbox/Code/mintime/robots/hrp4r.dae')
env.Load('/home/cuong/Dropbox/Code/mintime/robots/floorwalls.xml')
collisionChecker = RaveCreateCollisionChecker(env, 'pqp')
env.SetCollisionChecker(collisionChecker)
robot = env.GetRobots()[0]
hrp = HRP4.HRP4robot(robot)
n = robot.GetDOF()
g = 9.8
env.GetPhysicsEngine().SetGravity([0, 0, -g])
# DOF and velocity limits
dof_lim = robot.GetDOFLimits()
vel_lim = robot.GetDOFVelocityLimits()
robot.SetDOFLimits(-3 * ones(n), 3 * ones(n))
robot.SetDOFVelocityLimits(100 * vel_lim)
exclude_list = []
for k in range(n):
if robot.GetLinks()[k].GetMass() < 0.01:
exclude_list.append(k)
