def __init__(self, name=None, load=True):
Parent.__init__(self, load)
if load:
self.loadFullBodyModel(self.urdfName, self.rootJointType,
self.meshPackageName, self.packageName,
self.urdfSuffix, self.srdfSuffix)
if name != None:
self.name = name
def __init__(self, name=None, load=True):
Parent.__init__(self, load)
if load:
self.loadFullBodyModel(self.urdfName, self.rootJointType,
self.meshPackageName, self.packageName,
self.urdfSuffix, self.srdfSuffix)
if name is not None:
self.name = name
self.joint1L_bounds_prev = self.getJointBounds('leg_left_1_joint')
self.joint6L_bounds_prev = self.getJointBounds('leg_left_6_joint')
self.joint2L_bounds_prev = self.getJointBounds('leg_left_2_joint')
self.joint1R_bounds_prev = self.getJointBounds('leg_right_1_joint')
self.joint6R_bounds_prev = self.getJointBounds('leg_right_6_joint')
self.joint2R_bounds_prev = self.getJointBounds('leg_right_2_joint')
def __init__(self, name=None, load=True, client=None, clientRbprm=None):
if name is not None:
self.name = name
Parent.__init__(self, self.name, self.rootJointType, load, client,
None, clientRbprm)
# Even though the bound in the urdf is greater than this values,
# the controller do not tolerate values outside of this bounds
self.setJointBounds("leg_left_5_joint", [-1.26, 0.768])
self.setJointBounds("leg_right_5_joint", [-1.26, 0.768])
# Save urdf values for the bounds that may be modified
self.joint1L_bounds_prev = self.getJointBounds("leg_left_1_joint")
self.joint6L_bounds_prev = self.getJointBounds("leg_left_6_joint")
self.joint2L_bounds_prev = self.getJointBounds("leg_left_2_joint")
self.joint1R_bounds_prev = self.getJointBounds("leg_right_1_joint")
self.joint6R_bounds_prev = self.getJointBounds("leg_right_6_joint")
self.joint2R_bounds_prev = self.getJointBounds("leg_right_2_joint")
def __init__(self, name=None, load=True, client=None, clientRbprm=None):
if name is not None:
self.name = name
Parent.__init__(self, self.name, self.rootJointType, load, client, None, clientRbprm)
# save original bounds of the urdf for futur reset
self.FL_HAA_bounds = self.getJointBounds('FL_HAA')
self.FL_HFE_bounds = self.getJointBounds('FL_HFE')
self.FL_KFE_bounds = self.getJointBounds('FL_KFE')
self.FR_HAA_bounds = self.getJointBounds('FR_HAA')
self.FR_HFE_bounds = self.getJointBounds('FR_HFE')
self.FR_KFE_bounds = self.getJointBounds('FR_KFE')
self.HL_HAA_bounds = self.getJointBounds('HL_HAA')
self.HL_HFE_bounds = self.getJointBounds('HL_HFE')
self.HL_KFE_bounds = self.getJointBounds('HL_KFE')
self.HR_HAA_bounds = self.getJointBounds('HR_HAA')
self.HR_HFE_bounds = self.getJointBounds('HR_HFE')
self.HR_KFE_bounds = self.getJointBounds('HR_KFE')
def loadRobot(packageName,meshPackageName,rootJointType,urdfName,urdfSuffix,srdfSuffix, limbId, limbRoot, limbEffector):
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [0,0,0,0,0,0])
limbOffset = [0,0,0] #inutile ici
limbNormal = [0,1,0] #inutile ici
limbx = 0.09; limby = 0.05 #inutile ici
fullBody.addLimb(limbId,limbRoot,'',limbOffset,limbNormal, limbx, limby, 1000, "manipulability", 0.1)
return fullBody
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.gepetto import Viewer
import stair_bauzil_hrp2_path as tp
packageName = "hrp2_14_description"
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,
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.gepetto import Viewer
import car_hrp2_path as tp
packageName = "hrp2_14_description"
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", [-1, 2, -2, 1, 0.5, 2.5])
ps = tp.ProblemSolver(fullBody)
r = tp.Viewer(ps)
#~ AFTER loading obstacles
#~ AFTER loading obstacles
rLegId = '0rLeg'
rLeg = 'RLEG_JOINT0'
rLegOffset = [
0,
-0.105,
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
from hpp.gepetto import Viewer
import ground_crouch_hyq_path as tp
packageName = "hyq_description"
meshPackageName = "hyq_description"
rootJointType = "freeflyer"
##
# Information to retrieve urdf and srdf files.
urdfName = "hyq"
urdfSuffix = ""
srdfSuffix = ""
fullBody = FullBody()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName,
packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds("base_joint_xyz", [-6, 5, -4, 4, 0.6, 2])
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
nbSamples = 20000
ps = tp.ProblemSolver(fullBody)
r = tp.Viewer(ps)
rootName = 'base_joint_xyz'
# Creating limbs
from numpy import array, matrix
import random
import sample_com_vel as scv
packageName = "hrp2_14_description"
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 = ProblemSolver( fullBody )
r = Viewer (ps)
n_samples = 10000
#~ AFTER loading obstacles
rLegId = '0rLeg'
rLeg = 'RLEG_JOINT0'
rLegOffset = [0,-0.105,0,]
rLegNormal = [0,1,0]
def __init__ (self, name = None,load = True):
Parent.__init__ (self,load)
if load:
self.loadFullBodyModel(self.urdfName, self.rootJointType, self.meshPackageName, self.packageName, self.urdfSuffix, self.srdfSuffix)
if name != None:
self.name = name
# Importing helper class for RBPRM
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
import quaternion as quat
import numpy as np
packageName = "talos_description"
meshPackageName = "talos_description"
rootJointType = "freeflyer"
##
# Information to retrieve urdf and srdf files.
urdfName = "talos"
urdfSuffix = "_full_v2"
srdfSuffix = ""
# This time we load the full body model of HyQ
fullBody = FullBody()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName,
packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds("base_joint_xyz", [-20, 20, -20, 20, -20, 20])
rootName = "base_joint_xyz"
nbSamples = 100000
q_0 = [
0,
0,
0,
1,
0,
0,
0, # root_joint
from hpp.corbaserver.rbprm.rbprmbuilder import Builder from hpp.corbaserver.rbprm.rbprmfullbody import FullBody from hpp.gepetto import Viewer import ground_crouch_hyq_path as tp packageName = "hyq_description" meshPackageName = "hyq_description" rootJointType = "freeflyer" ## # Information to retrieve urdf and srdf files. urdfName = "hyq" urdfSuffix = "" srdfSuffix = "" fullBody = FullBody () fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix) from hpp.corbaserver.rbprm.problem_solver import ProblemSolver nbSamples = 20000 ps = tp.ProblemSolver( fullBody ) r = tp.Viewer (ps) rootName = 'base_joint_xyz' #~ AFTER loading obstacles cType = "_3_DOF" rLegId = 'rfleg'
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
from hpp.gepetto import Viewer
import sys
packageName = "hrp2_14_description"
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", [-1,2, -2, 1, 0.5, 2.5])
#~ AFTER loading obstacles
rLegId = 'hrp2_rleg_rom'
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.03)
lLegId = 'hrp2_lleg_rom'
lLeg = 'LLEG_JOINT0'
lLegOffset = [0,-0.105,0]
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.gepetto import Viewer
packageName = "hrp2_14_description"
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", [-1,1, -4, -1, 1, 2.2])
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
nbSamples = 50000
ps = ProblemSolver( fullBody )
#~ AFTER loading obstacles
rLegId = '7rLeg'
rLeg = 'RLEG_JOINT0'
rLegOffset = [0,-0.105,0,]
rLegNormal = [0,1,0]
rLegx = 0.09; rLegy = 0.05
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody from hpp.gepetto import Viewer import quaternion as quat packageName = "phantomx_description" meshPackageName = "phantomx_description" rootJointType = "freeflyer" ## # Information to retrieve urdf and srdf files. urdfName = "phantomx" urdfSuffix = "" srdfSuffix = "" fullBody = FullBody() fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix) from hpp.corbaserver.rbprm.problem_solver import ProblemSolver nbSamples = 50000 ps = ProblemSolver(fullBody) rootName = "base_joint_xyz" # ~ AFTER loading obstacles rmId = "rm" rm = "j_c1_rm" rme = "j_tibia_rm"
from os import environ
ins_dir = environ['DEVEL_DIR']
db_dir = ins_dir+"/install/share/hyq-rbprm/database/hyq_"
import ground_crouch_hyq_path as tp
packageName = "hyq_description"
meshPackageName = "hyq_description"
rootJointType = "freeflyer"
##
# Information to retrieve urdf and srdf files.
urdfName = "hyq"
urdfSuffix = ""
srdfSuffix = ""
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [-6,5, -4, 4, 0.6, 2])
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
nbSamples = 20000
ps = tp.ProblemSolver( fullBody )
r = tp.Viewer (ps)
rootName = 'base_joint_xyz'
# Creating limbs
# cType is "_3_DOF": positional constraint, but no rotation (contacts are punctual)
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.corbaserver.rbprm.tools.plot_analytics import plotOctreeValues
packageName = "anymal_bedi_description"
meshPackageName = "anymal_bedi_description"
rootJointType = "freeflyer"
# Information to retrieve urdf and srdf files.
urdfName = "anymal_bedi"
urdfSuffix = ""
srdfSuffix = ""
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("root_joint", [0,0,0,0,0,0])
nbSamples = 50000 #used to be 20000
cType = "_3_DOF"
rLegId = 'rfleg'
rLeg = 'RF_HAA'
rfoot = 'RF_MOUNT_TO_FOOT'
offset = [0.,-0.034625,0.] #originally [0.,-0.031,0.]
normal = [0,1,0] #hyq needs [0,1,0], also for anymal
legx = 0.03; legy = 0.03
def addLimbDb(limbId, heuristicName, loadValues = True, disableEffectorCollision = False):
fullBody.addLimbDatabase(str(db_dir+limbId+'.db'), limbId, heuristicName,loadValues, disableEffectorCollision)
#fullBody.addLimb(rLegId,rLeg,rfoot,offset,normal, legx, legy, nbSamples, "forward", 0.1, cType)
from hpp.gepetto import Viewer
import truck_hrp2_path as tp
packageName = "hrp2_14_description"
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,2.2, -1, 1, 0.7, 2.5])
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.03)
from hpp.gepetto import Viewer
import polaris_hrp2_path_3 as tp
packageName = "hrp2_14_description"
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", [-1,2, -2, 1, 0.5, 2.5])
ps = tp.ProblemSolver( fullBody )
r = tp.Viewer (ps)
#~ AFTER loading obstacles
#~ AFTER loading obstacles
rLegId = 'hrp2_rleg_rom'
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, "static", 0.03)
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.corbaserver.rbprm.tools.plot_analytics import plotOctreeValues
packageName = "anymal_bedi_description"
meshPackageName = "anymal_bedi_description"
rootJointType = "freeflyer"
# Information to retrieve urdf and srdf files.
urdfName = "anymal_bedi"
urdfSuffix = ""
srdfSuffix = ""
fullBody = FullBody()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName,
packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds("root_joint", [0, 0, 0, 0, 0, 0])
nbSamples = 50000 #used to be 20000
cType = "_3_DOF"
rLegId = 'rfleg'
rLeg = 'RF_HAA'
rfoot = 'RF_MOUNT_TO_FOOT'
offset = [0., -0.034625, 0.] #originally [0.,-0.031,0.]
normal = [0, 1, 0] #hyq needs [0,1,0], also for anymal
legx = 0.03
legy = 0.03
def addLimbDb(limbId,
heuristicName,
import quaternion as quat
packageName = 'hpp-rbprm-corba'
meshPackageName = 'hpp-rbprm-corba'
rootJointType = "freeflyer"
##
# Information to retrieve urdf and srdf files.
urdfName = "spiderman"
urdfSuffix = ""
srdfSuffix = ""
fullBody = FullBody ()
robot = fullBody.client.basic.robot
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [-20,20, -20, 20, -20, 20])
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
nbSamples = 1
ps = ProblemSolver( fullBody )
rLegId = 'RFoot'
lLegId = 'LFoot'
rarmId = 'RHand'
import downSlope_easy_hrp2_pathKino as tp
import time
packageName = "hrp2_14_description"
meshPackageName = "hrp2_14_description"
rootJointType = "freeflyer"
##
# Information to retrieve urdf and srdf files.
urdfName = "hrp2_14"
urdfSuffix = "_reduced"
srdfSuffix = ""
pId = tp.ps.numberPaths() -1
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [-2,5, 0, 2, 0.45, 1.8])
fullBody.client.basic.robot.setDimensionExtraConfigSpace(tp.extraDof)
ps = tp.ProblemSolver( fullBody )
ps.client.problem.setParameter("aMax",tp.aMax)
ps.client.problem.setParameter("vMax",tp.vMax)
r = tp.Viewer (ps,viewerClient=tp.r.client)
#~ AFTER loading obstacles
rLegId = 'hrp2_rleg_rom'
rLeg = 'RLEG_JOINT0'
rLegOffset = [0,0,-0.105]
rLegNormal = [0,0,1]
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.gepetto import Viewer
import josephpath as tp
packageName = "hrp2_14_description"
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", [-1,2, -1, 1, 0, 2.2])
ps = tp.ProblemSolver( fullBody )
r = tp.Viewer (ps)
r.loadObstacleModel ('hpp-rbprm-corba', "ground", "contact")
#~ AFTER loading obstacles
rLegId = '0rLeg'
rLeg = 'RLEG_JOINT0'
rLegOffset = [0,-0.105,0,]
rLegNormal = [0,1,0]
rLegx = 0.09; rLegy = 0.05
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody from hpp.gepetto import Viewer import quaternion as quat packageName = "hyq_description" meshPackageName = "hyq_description" rootJointType = "freeflyer" ## # Information to retrieve urdf and srdf files. urdfName = "hyq" urdfSuffix = "" srdfSuffix = "" fullBody = FullBody () fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix) from hpp.corbaserver.rbprm.problem_solver import ProblemSolver nbSamples = 50000 ps = ProblemSolver( fullBody ) rootName = 'base_joint_xyz' #~ AFTER loading obstacles rLegId = 'rfleg' rLeg = 'rf_haa_joint' rfoot = 'rf_foot_joint'
import time import omniORB.any from display_tools import * from hpp.corbaserver.rbprm.rbprmstate import State, StateHelper packageName = "hrp2_14_description" meshPackageName = "hrp2_14_description" rootJointType = "freeflyer" ## # Information to retrieve urdf and srdf files. urdfName = "hrp2_14" #~ urdfSuffix = "_reduced_safe" urdfSuffix = "_reduced" srdfSuffix = "" pId = tp.ps.numberPaths() - 1 fullBody = FullBody() tPlanning = 0. rleg = 'RLEG_JOINT0' rfoot = "RLEG_JOINT5" lleg = 'LLEG_JOINT0' lfoot = "LLEG_JOINT5" rarm = "RARM_JOINT0" rhand = "RARM_JOINT5" larm = "LARM_JOINT0" lhand = "LARM_JOINT5" rLegId = 'hrp2_rleg_rom' lLegId = 'hrp2_lleg_rom' rArmId = 'hrp2_rarm_rom' lArmId = 'hrp2_larm_rom'
from os import environ
ins_dir = environ['DEVEL_DIR']
db_dir = ins_dir + "/install/share/hyq-rbprm/database/hyq_"
packageName = "hyq_description"
meshPackageName = "hyq_description"
rootJointType = "freeflyer"
# Information to retrieve urdf and srdf files.
urdfName = "hyq"
urdfSuffix = ""
srdfSuffix = ""
# This time we load the full body model of HyQ
fullBody = FullBody()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName,
packageName, urdfSuffix, srdfSuffix)
fullBody.client.basic.robot.setDimensionExtraConfigSpace(tp.extraDof)
fullBody.setJointBounds("base_joint_xyz", [0.8, 5.6, -0.5, 0.5, 0.4, 1.2])
# Setting a number of sample configurations used
dynamic = True
ps = tp.ProblemSolver(fullBody)
r = tp.Viewer(ps,
viewerClient=tp.r.client,
displayArrows=True,
displayCoM=True)
# Setting a number of sample configurations used
nbSamples = 20000
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.gepetto import Viewer
import quaternion as quat
packageName = "hrp2_14_description"
meshPackageName = "hrp2_14_description"
rootJointType = "freeflyer"
##
# Information to retrieve urdf and srdf files.
urdfName = "hrp2_14"
urdfSuffix = "_reduced"
srdfSuffix = ""
fullBody = FullBody ()
#~ fullBody.setJointBounds ("base_joint_xyz", [-2,2, -2, 2, -2, 2])
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [-20,20, -20, 20, -20, 20])
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
nbSamples = 1
ps = ProblemSolver( fullBody )
rootName = 'base_joint_xyz'
rLegId = 'RL'
rLeg = 'RLEG_JOINT0'
from os import environ
ins_dir = environ['DEVEL_DIR']
db_dir = ins_dir+"/install/share/hyq-rbprm/database/hyq_"
packageName = "hyq_description"
meshPackageName = "hyq_description"
rootJointType = "freeflyer"
# Information to retrieve urdf and srdf files.
urdfName = "hyq"
urdfSuffix = ""
srdfSuffix = ""
# This time we load the full body model of HyQ
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [-2,5, -1, 1, 0.3, 4])
# Setting a number of sample configurations used
nbSamples = 20000
ps = tp.ProblemSolver(fullBody)
r = tp.Viewer (ps)
rootName = 'base_joint_xyz'
# Creating limbs
# cType is "_3_DOF": positional constraint, but no rotation (contacts are punctual)
cType = "_3_DOF"
# string identifying the limb
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.gepetto import Viewer
import quaternion as quat
packageName = "hyq_description"
meshPackageName = "hyq_description"
rootJointType = "freeflyer"
# Information to retrieve urdf and srdf files.
urdfName = "hyq"
urdfSuffix = "_6D"
srdfSuffix = ""
# This time we load the full body model of HyQ
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [-20,20, -20, 20, -20, 20])
# Setting a number of sample configurations used
nbSamples = 100000
rootName = 'base_joint_xyz'
# Creating limbs
# cType is "_3_DOF": positional constraint, but no rotation (contacts are punctual)
cType = "_6_DOF"
# string identifying the limb
rLegId = 'rfleg'
# First joint of the limb, as in urdf file
rLeg = 'rf_haa_joint'
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
from hpp.gepetto import Viewer
import sys
packageName = "hrp2_14_description"
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", [-1,2, -2, 1, 0.5, 2.5])
#~ AFTER loading obstacles
rLegId = 'hrp2_rleg_rom'
rLeg = 'RLEG_JOINT0'
rLegOffset = [
0,
-0.105,
0,
]
rLegNormal = [0, 1, 0]
rLegx = 0.09
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.gepetto import Viewer
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
packageName = "hrp2_14_description"
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)
ps = ProblemSolver(fullBody)
r = Viewer(ps)
justin = "/home/stonneau/dev/justin/hrp2-motions/standup/"
def check(csv):
qs = []
file = open(justin + csv, "r+")
# first retrieve frame range
for line in file.readlines():
objs = line.rstrip("\n").split(" ")
objs.pop(0)
import spiderman_backJump_path as tp
packageName = 'hpp-rbprm-corba'
meshPackageName = 'hpp-rbprm-corba'
rootJointType = "freeflyer"
##
# Information to retrieve urdf and srdf files.
urdfName = "spiderman"
urdfSuffix = ""
srdfSuffix = ""
V0list = tp.V0list
Vimplist = tp.Vimplist
base_joint_xyz_limits = tp.base_joint_xyz_limits
fullBody = FullBody()
robot = fullBody.client.basic.robot
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName,
packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds("base_joint_xyz", base_joint_xyz_limits)
#psf = ProblemSolver(fullBody); rr = Viewer (psf); gui = rr.client.gui
r = tp.r
ps = tp.ps
psf = tp.ProblemSolver(fullBody)
rr = tp.Viewer(psf)
gui = rr.client.gui
pp = PathPlayer(fullBody.client.basic, rr)
pp.speed = 0.6
q_0 = fullBody.getCurrentConfig()
rr(q_0)
import flatGround_hrp2_pathKino as tp
import time
tPlanning = tp.tPlanning
packageName = "hrp2_14_description"
meshPackageName = "hrp2_14_description"
rootJointType = "freeflyer"
##
# Information to retrieve urdf and srdf files.
urdfName = "hrp2_14"
urdfSuffix = "_reduced"
srdfSuffix = ""
pId = tp.ps.numberPaths() -1
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [-5,5, -1.5, 1.5, 0.5, 0.8])
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",tp.aMax)
ps.client.problem.setParameter("vMax",tp.vMax)
r = tp.Viewer (ps,viewerClient=tp.r.client,displayArrows = True, displayCoM = True)
q_init =[0.1, -0.82, 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)
qfar=q_ref[::]
from hpp.corbaserver.rbprm.tools.cwc_trajectory import *
from hpp import Error as hpperr
from numpy import array, matrix
packageName = "hrp2_14_description"
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 = ProblemSolver(fullBody)
r = Viewer(ps)
#~ AFTER loading obstacles
rLegId = '0rLeg'
rLeg = 'RLEG_JOINT0'
rLegOffset = [
0,
-0.105,
0,
from hpp.gepetto import Viewer #calling script darpa_hyq_path to compute root path import darpa_hyq_path as tp packageName = "hyq_description" meshPackageName = "hyq_description" rootJointType = "freeflyer" # Information to retrieve urdf and srdf files. urdfName = "hyq" urdfSuffix = "" srdfSuffix = "" # This time we load the full body model of HyQ fullBody = FullBody () fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix) # Setting a number of sample configurations used nbSamples = 20000 ps = tp.ProblemSolver(fullBody) r = tp.Viewer (ps) rootName = 'base_joint_xyz' # Creating limbs # cType is "_3_DOF": positional constraint, but no rotation (contacts are punctual) cType = "_3_DOF" # string identifying the limb rLegId = 'rfleg'
ins_dir = environ['DEVEL_DIR']
db_dir = ins_dir + "/install/share/hyq-rbprm/database/hyq_"
from hpp.corbaserver import Client
packageName = "hyq_description"
meshPackageName = "hyq_description"
rootJointType = "freeflyer"
# Information to retrieve urdf and srdf files.
urdfName = "hyq"
urdfSuffix = ""
srdfSuffix = ""
# This time we load the full body model of HyQ
fullBody = FullBody()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName,
packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds("base_joint_xyz", [-4, 6, -1, 1, 0.3, 2.5])
# Setting a number of sample configurations used
nbSamples = 20000
ps = tp.ProblemSolver(fullBody)
r = tp.Viewer(ps, viewerClient=tp.r.client)
rootName = 'base_joint_xyz'
cType = "_3_DOF"
rLegId = 'rfleg'
rLeg = 'rf_haa_joint'
import time
#from constraint_to_dae import *
from hpp.corbaserver.rbprm.rbprmstate import State, StateHelper
#from display_tools import *
import talos.airbus_no_plane_bigStairs_path as tp
import time
from planning.robot_config.talos import *
from display_tools import *
tPlanning = tp.tPlanning
##
# Information to retrieve urdf and srdf files.
pId = tp.ps.numberPaths() - 1
fullBody = FullBody()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName,
packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds("base_joint_xyz", [-13, 10, -3.85, -3.75, -1.75, 0.5])
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", tp.aMax)
ps.client.problem.setParameter("vMax", tp.vMax)
r = tp.Viewer(ps, viewerClient=tp.r.client, displayCoM=True)
q_ref = [
0.0,
0.0,
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.gepetto import Viewer
import downSlope_hrp2_pathKino2 as tp
import time
packageName = "hrp2_14_description"
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, 0, 2, 0.45, 1.8])
fullBody.client.basic.robot.setDimensionExtraConfigSpace(tp.extraDof)
ps = tp.ProblemSolver(fullBody)
ps.client.problem.setParameter("aMax", tp.aMax)
ps.client.problem.setParameter("vMax", tp.vMax)
r = tp.Viewer(ps, viewerClient=tp.r.client)
#~ AFTER loading obstacles
rLegId = 'hrp2_rleg_rom'
rLeg = 'RLEG_JOINT0'
rLegOffset = [0, 0, -0.105]
ins_dir = environ['DEVEL_DIR']
db_dir = ins_dir+"/install/share/hyq-rbprm/database/hyq_"
pathId = tp.ps.numberPaths()-1
packageName = "hyq_description"
meshPackageName = "hyq_description"
rootJointType = "freeflyer"
# Information to retrieve urdf and srdf files.
urdfName = "hyq"
urdfSuffix = ""
srdfSuffix = ""
# This time we load the full body model of HyQ
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.client.basic.robot.setDimensionExtraConfigSpace(tp.extraDof)
fullBody.setJointBounds ("base_joint_xyz", [-1.25,5, 0, 2, 0.3, 1.8])
# Setting a number of sample configurations used
nbSamples = 20000
dynamic=True
ps = tp.ProblemSolver(fullBody)
ps.client.problem.setParameter("aMax",tp.aMax)
ps.client.problem.setParameter("vMax",tp.vMax)
r = tp.Viewer (ps,viewerClient=tp.r.client)
rootName = 'base_joint_xyz'
from hpp.gepetto import Viewer
from tools import *
import bauzilStairs_hrp2_pathKino as tp
import time
import omniORB.any
packageName = "hrp2_14_description"
meshPackageName = "hrp2_14_description"
rootJointType = "freeflyer"
##
# Information to retrieve urdf and srdf files.
urdfName = "hrp2_14"
urdfSuffix = "_reduced"
srdfSuffix = ""
pId = tp.ps.numberPaths() - 1
fullBody = FullBody()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName,
packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds("base_joint_xyz", [-5.5, 5.5, -2.5, 2.5, 0.3, 0.65])
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))
r = tp.Viewer(ps,
viewerClient=tp.r.client,
displayArrows=True,
displayCoM=True)
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.gepetto import Viewer
import plane_hrp2_path as tp
import time
packageName = "hrp2_14_description"
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, viewerClient=tp.r.client)
#~ AFTER loading obstacles
rLegId = '0rLeg'
rLeg = 'RLEG_JOINT0'
rLegOffset = [
0,
-0.105,
0,
from hpp.gepetto import Viewer
import time
packageName = "hrp2_14_description"
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)
#~ AFTER loading obstacles
rLegId = 'hrp2_rleg_rom'
rLeg = 'RLEG_JOINT0'
rLegOffset = [0,0,-0.105]
rLegNormal = [0,0,1]
rLegx = 0.09; rLegy = 0.05
fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 10000, "manipulability", 0.1)
lLegId = 'hrp2_lleg_rom'
lLeg = 'LLEG_JOINT0'
from hpp.gepetto import Viewer
import stair_bauzil_hrp2_path as tp
packageName = "hrp2_14_description"
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)
import plateform_hrp2_path as tp
import time
tPlanning = tp.tPlanning
packageName = "hrp2_14_description"
meshPackageName = "hrp2_14_description"
rootJointType = "freeflyer"
##
# Information to retrieve urdf and srdf files.
urdfName = "hrp2_14"
urdfSuffix = "_reduced"
srdfSuffix = ""
pId = tp.ps.numberPaths() -1
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [-1,2, -0.5, 0.5, 0.5, 0.8])
fullBody.client.basic.robot.setDimensionExtraConfigSpace(tp.extraDof)
fullBody.client.basic.robot.setExtraConfigSpaceBounds([-1,1,-1,1,-0.5,0.5,0,0,0,0,0,0])
ps = tp.ProblemSolver( fullBody )
ps.client.problem.setParameter("aMax",tp.aMax)
ps.client.problem.setParameter("vMax",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)
import polaris_hrp2_path_no_step as tp
from numpy import array, matrix
import numpy as np
packageName = "hrp2_14_description"
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", [-1,2, -2, 1, 0.5, 2.5])
ps = tp.ProblemSolver( fullBody )
r = tp.Viewer (ps)
#~ AFTER loading obstacles
#~ AFTER loading obstacles
rLegId = 'hrp2_rleg_rom'
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.03, "_6_DOF")
ins_dir = environ['DEVEL_DIR']
db_dir = ins_dir + "/install/share/hyq-rbprm/database/hyq_"
from hpp.corbaserver import Client
packageName = "hyq_description"
meshPackageName = "hyq_description"
rootJointType = "freeflyer"
# Information to retrieve urdf and srdf files.
urdfName = "hyq"
urdfSuffix = ""
srdfSuffix = ""
# This time we load the full body model of HyQ
fullBody = FullBody()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName,
packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds("root_joint", [-2, 5, -1, 1, 0.3, 4])
# Setting a number of sample configurations used
nbSamples = 10000
ps = ProblemSolver(fullBody)
r = Viewer(ps)
q_init = hyq_ref[:]
r(q_init)
from hppy_ik import *
import flatGround_hrp2_pathKino as tp
import time
tPlanning = tp.tPlanning
packageName = "hrp2_14_description"
meshPackageName = "hrp2_14_description"
rootJointType = "freeflyer"
##
# Information to retrieve urdf and srdf files.
urdfName = "hrp2_14"
urdfSuffix = "_reduced_kinematics"
srdfSuffix = ""
pId = tp.ps.numberPaths() -1
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [-5,5, -1.5, 1.5, 0.5, 0.8])
fullBody.client.basic.robot.setDimensionExtraConfigSpace(tp.extraDof)
ps = tp.ProblemSolver( fullBody )
ps.client.problem.setParameter("aMax",tp.aMax)
ps.client.problem.setParameter("vMax",tp.vMax)
r = tp.Viewer (ps,viewerClient=tp.r.client,displayArrows = True, displayCoM = True)
q_init =[0.1, -0.82, 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)
ins_dir = environ['DEVEL_DIR']
db_dir = ins_dir + "/install/share/hyq-rbprm/database/hyq_"
pathId = tp.ps.numberPaths() - 1
packageName = "hyq_description"
meshPackageName = "hyq_description"
rootJointType = "freeflyer"
# Information to retrieve urdf and srdf files.
urdfName = "hyq"
urdfSuffix = ""
srdfSuffix = ""
# This time we load the full body model of HyQ
fullBody = FullBody()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName,
packageName, urdfSuffix, srdfSuffix)
fullBody.client.basic.robot.setDimensionExtraConfigSpace(tp.extraDof)
fullBody.setJointBounds("base_joint_xyz", [-2.5, 2.5, -6, 6, 0.6, 0.65])
# Setting a number of sample configurations used
nbSamples = 20000
dynamic = True
ps = tp.ProblemSolver(fullBody)
ps.client.problem.setParameter("aMax", tp.aMax)
ps.client.problem.setParameter("vMax", tp.vMax)
r = tp.Viewer(ps, viewerClient=tp.r.client)
rootName = 'base_joint_xyz'
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody from hpp.gepetto import Viewer import quaternion as quat packageName = "hrp2_14_description" 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) from hpp.corbaserver.rbprm.problem_solver import ProblemSolver nbSamples = 50000 ps = ProblemSolver( fullBody ) rootName = 'base_joint_xyz' #~ AFTER loading obstacles rLegId = 'rLeg' rLeg = 'RLEG_JOINT0' rfoot = 'RLEG_JOINT5'
def __init__(self, name=None, load=True, client=None, clientRbprm=None):
if name is not None:
self.name = name
Parent.__init__(self, self.name, self.rootJointType, load, client,
None, clientRbprm)
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.gepetto import Viewer
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
packageName = "hrp2_14_description"
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)
ps = ProblemSolver( fullBody )
r = Viewer (ps)
justin = "/home/stonneau/dev/justin/hrp2-motions/standup/"
def check(csv):
qs=[]
file = open(justin+csv,"r+");
# first retrieve frame range
for line in file.readlines():
objs = line.rstrip("\n").split(" ");
objs.pop(0)
q = fullBody.getCurrentConfig();
for i in range (0, len(q)-1):