Python FullBody Examples

Example #1

File: robot.py Project: nim65s/hpp-rbprm-robot-data

 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

Example #2

File: talos_fixedUpper.py Project: pFernbach/talos-rbprm

 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')

Example #3

    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")

Example #4

    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')

Example #5

File: olivier_urdf.py Project: airobert/hpp-rbprm-corba

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

Example #6

File: stair_bauzil_hrp2_interp.py Project: rohanpsingh/hpp-rbprm-corba

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,

Example #7

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,

Example #8

File: ground_crouch_hyq_bridge.py Project: rohanpsingh/hpp-rbprm-corba

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

Example #9

File: gen_hrp2_statically_balanced_positions_2d.py Project: stonneau/hpp-rbprm-corba

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]

Example #10

File: robot.py Project: stonneau/hpp-rbprm-robot-data

 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

Example #11

# 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

Example #12

File: ground_crouch_hyq_interp.py Project: anna-seppala/hpp-rbprm-corba

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'

Example #13

File: 3_equilibrium_feasibility.py Project: stonneau/hpp-rbprm-corba

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]

Example #14

File: generateROMs.py Project: airobert/hpp-rbprm-corba

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

Example #15

File: generateROMsPhantom.py Project: pFernbach/hpp-rbprm-corba

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"

Example #16

File: ground_crouch_hyq_interp.py Project: stonneau/hpp-rbprm-corba

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)

Example #17

File: anymal_data.py Project: stonneau/hpp-rbprm-robot-data

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)

Example #18

File: truck_hrp2_interp.py Project: airobert/hpp-rbprm-corba

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)

Example #19

File: poralis_hrp2_interp_3.py Project: teguhSL/hpp-rbprm-corba

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)

Example #20

File: anymal_data.py Project: stonneau/hpp-rbprm-robot-data

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,

Example #21

File: admissibleComPositionsFromEffectorSpidey.py Project: stonneau/hpp-rbprm-robot-data

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'

Example #22

File: downSlope_easy_hrp2_interp.py Project: stonneau/hpp-rbprm-corba-1

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]

Example #23

File: josephload.py Project: pFernbach/hpp-rbprm-corba

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

Example #24

File: generateROMsHyq.py Project: airobert/hpp-rbprm-corba

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'

Example #25

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'

Example #26

File: sideWall_hyq_interpKino_fullBody.py Project: nim65s/hpp-rbprm-corba

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

Example #27

File: admissibleComPositionsFromEffector.py Project: stonneau/hpp-rbprm-corba

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'

Example #28

File: darpa_hyq_interp.py Project: stonneau/hpp-rbprm-corba

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

Example #29

File: admissibleComPositionsFromEffectorHyQ.py Project: stonneau/hpp-rbprm-robot-data

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'

Example #30

File: computespaces_blanced.py Project: stonneau/hpp-rbprm-corba-1

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

Example #31

File: check_motion.py Project: stonneau/hpp-rbprm-corba-1

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)

Example #32

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)

Example #33

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[::]

Example #34

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,

Example #35

File: darpa_hyq_interp.py Project: anna-seppala/hpp-rbprm-corba

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'

Example #36

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'

Example #37

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,

Example #38

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]

Example #39

File: downSlope_hyq_interpKino.py Project: teguhSL/hpp-rbprm-corba

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'

Example #40

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)

Example #41

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,

Example #42

File: hrp2_model.py Project: nim65s/hpp-rbprm-corba

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'                                                                     

Example #43

File: stair_bauzil_hrp2_interp.py Project: stonneau/hpp-rbprm-corba

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)

Example #44

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)

Example #45

File: polaris_hrp2_interp.py Project: stonneau/hpp-rbprm-corba

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")

Example #46

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 *

Example #47

File: flatGround_hrp2_interpSTATIC.py Project: teguhSL/hpp-rbprm-corba

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)

Example #48

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'

Example #49

File: admissibleRootPositionsFromEffector.py Project: airobert/hpp-rbprm-corba

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'

Example #50

File: hyq_contact6D.py Project: nim65s/hyq-rbprm

 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)

Example #51

File: check_motion.py Project: airobert/hpp-rbprm-corba

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):