Commit ed284623 authored by stonneau's avatar stonneau Committed by GitHub
Browse files

Merge pull request #3 from stonneau/master

release of rbprm-corba for ijrr resubmission
parents 2d4fce53 e40047bd
<robot name="polaris">
<link name="base_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://hpp-rbprm-corba/meshes/polaris.stl"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://hpp-rbprm-corba/meshes/polaris.stl"/>
</geometry>
</collision>
</link>
</robot>
<robot name="scene_wall2">
<link name="base_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://hpp-rbprm-corba/meshes/ground_table2.stl"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://hpp-rbprm-corba/meshes/ground_table2.stl"/>
</geometry>
</collision>
</link>
</robot>
......@@ -87,6 +87,12 @@ module hpp
in string urdfSuffix, in string srdfSuffix)
raises (Error);
/// Create a RbprmFullBody object
/// The device automatically has an anchor joint called "base_joint" as
/// root joint.
void loadFullBodyRobotFromExistingRobot ()
raises (Error);
/// Set Rom constraints for the configuration shooter
/// a configuration will only be valid if all roms indicated
/// are colliding with the environment.
......@@ -98,13 +104,11 @@ module hpp
/// Set Rom surface constraints for the configuration shooter
/// a Rom configuration will only be valid it collides with a surface
/// the normal of which is colinear to the indicated normal,
/// modulated by a range value.
/// \param normal prefered contact surface normal direction
/// \param range tolerance between surface normal and desired surface normal. If the dot
/// product of the normal is greater than range then the surface will be accepted.
///
void setNormalFilter(in string romName, in floatSeq normal, in double range) raises (Error);
/// that forms a given affordance (support, lean, etc.)
/// \param affordances a list of affordances accepted for
/// validation of given Rom
///
void setAffordanceFilter(in string romName, in Names_t affordances) raises (Error);
/// Sets limits on robot orientation, described according to Euler's ZYX rotation order
///
......@@ -113,6 +117,19 @@ module hpp
/// [z_inf, z_sup, y_inf, y_sup, x_inf, x_sup]
void boundSO3(in floatSeq limitszyx) raises (Error);
/// Project a state into a COM
///
/// \param stateId target state
/// \param com target com
double projectStateToCOM(in unsigned short stateId, in floatSeq com) raises (Error);
/// Create a state and push it to the state array
///
/// \param q configuration
/// \param names list of effectors in contact
/// \return stateId
short createState(in floatSeq configuration, in Names_t contactLimbs) raises (Error);
/// Get Sample configuration by its id
/// \param sampleName name of the limb from which to retrieve a sample
/// \param sampleId id of the desired samples
......@@ -128,6 +145,13 @@ module hpp
/// and the selected sample
floatSeq getSamplePosition(in string sampleName, in unsigned short sampleId) raises (Error);
/// Get the end effector position for a given configuration, assuming z normal
/// \param limbName name of the limb from which to retrieve a sample
/// \param dofArray configuration of the robot
/// \return world position of the limb end effector given the current robot configuration.
/// array of size 4, where each entry is the position of a corner of the contact surface
floatSeqSeq getEffectorPosition(in string limbName, in floatSeq dofArray) raises (Error);
/// Get the end effector position of a given limb configuration
/// \param limbName name of the limb from which to retrieve a sample
/// \return number of samples generated for the limb
......@@ -145,12 +169,24 @@ module hpp
/// \return the value computed for the given sample and analytics
double getSampleValue(in string limbName, in string valueName, in unsigned short sampleId) raises (Error);
/// compute the distance between all effectors replaced between two states
/// does not account for contact not present in both states
/// \param state1 from state
/// \param state2 destination state
/// \return the value computed for the given sample and analytics
double getEffectorDistance(in unsigned short state1, in unsigned short state2) raises (Error);
/// Generate all possible contact in a given configuration
/// \param dofArray initial configuration of the robot
/// \param direction desired direction of motion for the robot
/// \return transformed configuration for which all possible contacts have been created
floatSeq generateContacts(in floatSeq dofArray, in floatSeq direction) raises (Error);
/// Generate an autocollision free configuration with limbs in contact with the ground
/// \param contactLimbs name of the limbs to project in contact
/// \return a sampled contact configuration
floatSeq generateGroundContact(in Names_t contactLimbs) raises (Error);
/// Given a configuration and a limb, returns the id of all samples potentially in contact with the
/// environment, ordered by their efficiency
/// \param name name of the considered limb
......@@ -159,6 +195,15 @@ module hpp
/// \return transformed configuration for which all possible contacts have been created
floatSeq getContactSamplesIds(in string name, in floatSeq dofArray, in floatSeq direction) raises (Error);
/// Given a configuration and a limb, returns the id of all samples potentially in contact with the
/// environment, ordered by their efficiency
/// \param name name of the considered limb
/// \param dofArray considered configuration of the robot
/// \param direction desired direction of motion for the robot
/// \return transformed configuration for which all possible contacts have been created
floatSeqSeq getContactSamplesProjected(in string name, in floatSeq dofArray, in floatSeq direction,
in unsigned short numSamples) raises (Error);
/// get Ids of limb in an octree cell
/// \param name name of the considered limb
/// \param octreeNodeId considered configuration of the robot
......@@ -199,6 +244,11 @@ module hpp
/// \param contactLimbs ids of the limb in contact for the state
void setStartState(in floatSeq dofArray, in Names_t contactLimbs) raises (Error);
/// Compute effector contact points and normals for a given configuration
/// in local coordinates
/// \param dofArray configuration of the robot
/// \param limbName ids of the limb in contact
floatSeq computeContactForConfig(in floatSeq dofArray, in string limbName) raises (Error);
/// Set the end state of a contact generation problem
/// environment, ordered by their efficiency
......@@ -206,12 +256,26 @@ module hpp
/// \param contactLimbs ids of the limb in contact for the state
void setEndState(in floatSeq dofArray, in Names_t contactLimbs) raises (Error);
/// Provided a discrete contact sequence has already been computed, computes
/// all the contact positions and normals for a given state, the next one, and the intermediate between them.
/// \param stateId normalized step for generation along the path (ie the path has a length of 1).
/// \return list of 2 or 3 lists of 6d vectors [pox, poy, posz, nprmalx, normaly, normalz]
floatSeqSeq computeContactPoints(in unsigned short stateId) raises (Error);
/// Provided a discrete contact sequence has already been computed, computes
/// all the contact positions and normals for a given state, the next one, and the intermediate between them.
/// \param stateId normalized step for generation along the path (ie the path has a length of 1).
/// \return list of 2 or 3 lists of 6d vectors [pox, poy, posz, nprmalx, normaly, normalz]
floatSeqSeq computeContactPointsForLimb(in unsigned short stateId, in string limbname) raises (Error);
/// Provided a path has already been computed, interpolates it and generates the statically stable
/// constact configurations along it. setStartState and setEndState must have been called prior
/// to this function. If these conditions are not met an error is raised.
/// \param timestep normalized step for generation along the path (ie the path has a length of 1).
/// \param path path computed.
floatSeqSeq interpolate(in double timestep, in double path, in double robustnessTreshold) raises (Error);
/// \param robustnessTreshold minimum value of the static equilibrium robustness criterion required to accept the configuration (0 by default).
/// \param filterStates If different than 0, the resulting state list will be filtered to remove unnecessary states
floatSeqSeq interpolate(in double timestep, in double path, in double robustnessTreshold, in unsigned short filterStates) raises (Error);
/// Provided a path has already been computed, interpolates it and generates the statically stable
/// constact configurations along it. setStartState and setEndState must have been called prior
......@@ -219,7 +283,49 @@ module hpp
/// \param timestep normalized step for generation along the path (ie the path has a length of 1).
/// \param path path computed.
/// \param robustnessTreshold minimum value of the static equilibrium robustness criterion required to accept the configuration (0 by default).
floatSeqSeq interpolateConfigs(in floatSeqSeq configs, in double robustnessTreshold) raises (Error);
/// \param filterStates If different than 0, the resulting state list will be filtered to remove unnecessary states
floatSeqSeq interpolateConfigs(in floatSeqSeq configs, in double robustnessTreshold, in unsigned short filterStates) raises (Error);
/// returns the CWC of the robot at a given state
///
/// \param stateId The considered state
/// \param friction The friction coefficient
/// \return H matrix, such that H w <= h. h is added as the last column
floatSeqSeq getContactCone(in unsigned short stateId, in double friction) raises (Error);
/// returns the CWC of the robot between two states
///
/// \param stateId The considered state
/// \return H matrix, such that H w <= h. h is added as the last column
floatSeqSeq getContactIntermediateCone(in unsigned short stateId, in double friction) raises (Error);
/// Create a path for the root given
/// a set of 3d key points
/// The path is composed of n+1 linear interpolations
/// between the n positions.
/// The rotation is linearly interpolated as well,
/// between a start and a goal rotation. The resulting path
/// is added to the problem solver
/// \param positions array of positions
/// \param q1 quaternion of 1st rotation
/// \param q2 quaternion of 2nd rotation
/// \return id of the root path computed
short generateRootPath(in floatSeqSeq rootPositions, in floatSeq q1, in floatSeq q2) raises (Error);
/// Create a com trajectory given list of positions, velocities and accelerations
/// accelerations list contains one less element because it does not have an initial state.
/// a set of 3d key points
/// The path is composed of n+1 integrations
/// between the n positions.
/// The resulting path
/// is added to the problem solver
/// \param positions array of positions
/// \param velocities array of velocities
/// \param accelerations array of accelerations
/// \param dt time between two points
/// \return id of the root path computed
short generateComTraj(in floatSeqSeq positions, in floatSeqSeq velocities, in floatSeqSeq accelerations, in double dt) raises (Error);
/// Provided a path has already been computed and interpolated, generate a continuous path
/// between two indicated states. The states do not need to be consecutive, but increasing in Id.
......@@ -230,7 +336,8 @@ module hpp
/// \param state1 index of first state.
/// \param state2 index of second state.
/// \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states
void interpolateBetweenStates(in double state1, in double state2, in unsigned short numOptimizations) raises (Error);
/// \return id of the root path computed
short limbRRT(in double state1, in double state2, in unsigned short numOptimizations) raises (Error);
/// Provided a path has already been computed and interpolated, generate a continuous path
/// between two indicated states. The states do not need to be consecutive, but increasing in Id.
......@@ -242,7 +349,109 @@ module hpp
/// \param state2 index of second state.
/// \param path index of the path considered for the generation
/// \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states
void interpolateBetweenStatesFromPath(in double state1, in double state2, in unsigned short path, in unsigned short numOptimizations) raises (Error);
/// \return id of the root path computed
short limbRRTFromRootPath(in double state1, in double state2, in unsigned short path, in unsigned short numOptimizations) raises (Error);
/// Linear interpolation of many configurations into a path
/// \param configs list of configurations
/// \return id of the root path computed
short configToPath(in floatSeqSeq configs) raises (Error);
/// Provided a path has already been computed and interpolated, generate a continuous path
/// between two indicated states. The states do not need to be consecutive, but increasing in Id.
/// Will fail if the index of the states do not exist
/// The path of the COM of thr robot and limbs not considered by the contact transitions between
/// two states is assumed to be already computed, and registered in the solver under the id specified by the user.
/// It must be valid in the sense of the active PathValidation.
/// \param state1 index of first state.
/// \param state2 index of second state.
/// \param comPath index of the path considered of the com path
/// \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states
/// \return id of the root path computed
short comRRT(in double state1, in double state2, in unsigned short comPath, in unsigned short numOptimizations) raises (Error);
/// Provided a path has already been computed and interpolated, generate a continuous path
/// between two indicated states. The states do not need to be consecutive, but increasing in Id.
/// Will fail if the index of the states do not exist
/// The path of the COM of thr robot and limbs not considered by the contact transitions between
/// two states is assumed to be already computed, and registered in the solver under the id specified by the user.
/// It must be valid in the sense of the active PathValidation.
/// \param state1 index of first state.
/// \param rootPositions1 com positions to track for 1st phase
/// \param rootPositions1 com positions to track for 2nd phase
/// \param rootPositions1 com positions to track for 3rd phase
/// \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states
/// \return id of the root path computed
floatSeq comRRTFromPos(in double state1,
in unsigned short comTraj1,
in unsigned short comTraj2,
in unsigned short comTraj3,
in unsigned short numOptimizations) raises (Error);
/// Provided a path has already been computed and interpolated, generate a continuous path
/// between two indicated states. The states do not need to be consecutive, but increasing in Id.
/// Will fail if the index of the states do not exist
/// The path of the COM of thr robot and limbs not considered by the contact transitions between
/// two states is assumed to be already computed, and registered in the solver under the id specified by the user.
/// It must be valid in the sense of the active PathValidation.
/// \param state1 index of first state.
/// \param rootPositions1 com positions to track for 1st phase
/// \param rootPositions1 com positions to track for 2nd phase
/// \param rootPositions1 com positions to track for 3rd phase
/// \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states
/// \return id of the root path computed
floatSeq effectorRRT(in double state1,
in unsigned short comTraj1,
in unsigned short comTraj2,
in unsigned short comTraj3,
in unsigned short numOptimizations) raises (Error);
/// Provided a path has already been computed and interpolated, generate a continuous path
/// between two indicated states. The states do not need to be consecutive, but increasing in Id.
/// Will fail if the index of the states do not exist
/// The path of the COM of thr robot and limbs not considered by the contact transitions between
/// two states is assumed to be already computed, and registered in the solver under the id specified by the user.
/// It must be valid in the sense of the active PathValidation.
/// \param state1 index of first state.
/// \param rootPositions1 com positions to track for 1st phase
/// \param rootPositions1 com positions to track for 2nd phase
/// \param rootPositions1 com positions to track for 3rd phase
/// \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states
/// \return id of the root path computed
floatSeq effectorRRTFromPath(in double state1, in unsigned short refPath,
in double path_from, in double path_to,
in unsigned short comTraj1,
in unsigned short comTraj2,
in unsigned short comTraj3,
in unsigned short numOptimizations,
in Names_t trackedEffectors) raises (Error);
/// Project a given state into a given COM position
/// between two indicated states. The states do not need to be consecutive, but increasing in Id.
/// Will fail if the index of the state does not exist.
/// \param state index of first state.
/// \param targetCom 3D vector for the com position
/// \return projected configuration
floatSeq projectToCom(in double state, in floatSeq targetCom) raises (Error);
/// Retrieve the configuration at a given state
/// between two indicated states. The states do not need to be consecutive, but increasing in Id.
/// Will fail if the index of the state does not exist.
/// \param state index of first state.
/// \return projected configuration
floatSeq getConfigAtState(in unsigned short state) raises (Error);
/// \param limb name of the limb for which the request aplies
/// \param state1 current state considered
/// \return whether the limb is in contact at this state
short isLimbInContact(in string limbname, in double state1) raises (Error);
/// Is limb in contact during the motion from the current state to the next one
/// \param limb name of the limb for which the request aplies
/// \param state1 current state considered
/// \return whether the limb is in contact at this state
short isLimbInContactIntermediary(in string limbname, in double state1) raises (Error);
/// Saves the last computed states by the function interpolate in a filename.
/// Raises an error if interpolate has not been called, or the file could not be opened.
......@@ -294,6 +503,11 @@ module hpp
/// \return min and max values obtained
floatSeq runLimbSampleAnalysis(in string limbname, in string analysis, in double isstatic) raises (Error);
/// if the preprocessor variable PROFILE is active
/// dump the profiling data into a logFile
/// \param logFile name of the file where to dump the profiling data
void dumpProfile(in string logFile) raises (Error);
}; // interface Robot
}; // module rbprm
}; // module corbaserver
......
......@@ -2,20 +2,28 @@
import subprocess as sp
import os
import shutil
import datetime
import time
#~ scenarios = ['standing_hrp2']
scenarios = ['car_hrp2']
scenarios = ['ground_crouch_hyq']
#~ scenarios = ['stair_bauzil_hrp2']
n_trials = 100
n_trials = 200
stats = ['balance','collision','ik']
stats_optim = ['time_cwc','com_traj']
allstats = stats + stats_optim
python_script_extension = "_interp.py"
analysis = {}
lower = -100000
higher = 100000000
log_dir = "./logs"
if not os.path.exists(log_dir):
os.makedirs(log_dir)
def avg(l):
if(len(l) == 0):
return -1
......@@ -53,7 +61,8 @@ def parseLine(sep, name, line, data):
def parseData(scenario):
filename = scenario+"_log.txt";
os.rename("log.txt", filename)
os.rename("log.txt", filename)
shutil.move("./"+filename, log_dir+"/"+filename)
analysis[scenario] = {}
data = analysis[scenario]
data['no contact'] = 0
......@@ -67,9 +76,16 @@ def parseData(scenario):
data['path_time'] = []
data['min_path_time'] = higher
data['max_path_time'] = lower
for stat in stats:
#cwc data
data["optim_error_optim_fail"] = 0
data[ "optim_error_com_proj"] = 0
data[ "optim_error_unknown"] = 0
data[ "optim_num_success"] = 0
data[ "optim_num_trials"] = 0
data[ "num_errors"] = 0
for stat in allstats:
initdata(stat, data)
file = open(filename,"r+");
file = open(log_dir+"/"+filename,"r+");
for line in file.readlines():
if not (line.find('complete generation') == -1):
time = float(line.split()[3])
......@@ -97,6 +113,29 @@ def parseData(scenario):
data['path_time'].append(time);
data['min_path_time'] = min(data['min_path_time'],time)
data['max_path_time'] = max(data['max_path_time'],time)
#now, analyzing optimization
elif not (line.find('optim_error_optim_fail') == -1):
val = float(line.rstrip("\n").split()[1])
data['optim_error_optim_fail']+= val;
elif not (line.find('optim_error_com_proj') == -1):
val = float(line.rstrip("\n").split()[1])
data['optim_error_com_proj']+= val;
elif not (line.find('optim_error_unknown') == -1):
val = float(line.rstrip("\n").split()[1])
data['optim_error_unknown']+= val;
elif not (line.find('optim_num_success') == -1):
val = float(line.rstrip("\n").split()[1])
data['optim_num_success']+= val;
elif not (line.find('optim_num_trials') == -1):
val = float(line.rstrip("\n").split()[1])
data['optim_num_trials']+= val;
elif not (line.find('num_errors') == -1):
val = float(line.rstrip("\n").split()[1])
data['num_errors']+= val;
elif not (line.find('time_cwc') == -1):
parseLine(1, 'time_cwc', line, data)
elif not (line.find('com_traj') == -1):
parseLine(1, 'com_traj', line, data)
def printOneStat(f, name, data, g_time, tTime):
......@@ -111,9 +150,21 @@ def printOneStat(f, name, data, g_time, tTime):
f.write ("\t total time (min / avg / max / % of total / % of total wtht path planning):\n \t " + str(d['total_'+n+'_min']) + "\t" + str(d['total_'+n]) + "\t" + str(d['total_'+n+'_max']) + "\t" + str(float(d['total_'+n]) / tTime * 100) + "%" + "\t" + str(float(d['total_'+n]) / g_time * 100) + "%\n")
f.write ("\t number of tests (min / avg / max):\n \t " + str(d['minnum'+n]) + "\t" + str(d['num'+n]) + "\t" + str(d['maxnum'+n]) + "\n")
def printOneStatOptim(f, name, data):
n = name
d = data
print(n, len(d[n]))
d[n] = avg(d[n])
d['num'+n] = avg(d['num'+n])
d['total_'+n] = avg(d['total_'+n])
f.write (n +" tests: \n")
f.write ("\t single operation time (min / avg / max):\n \t " + str(d[n+'_min']) + "\t" + str(d[n]) + "\t" + str(d[n+'_max']) + "\t \n")
f.write ("\t total time (min / avg / max ):\n \t " + str(d['total_'+n+'_min']) + "\t" + str(d['total_'+n]) + "\t" + str(d['total_'+n+'_max']) + "\t" + "%\n")
f.write ("\t number of tests (min / avg / max):\n \t " + str(d['minnum'+n]) + "\t" + str(d['num'+n]) + "\t" + str(d['maxnum'+n]) + "\n")
def analyzeData():
f = open("log_"+str(datetime.datetime.now())+".txt","w+")
f = open(log_dir+"/log_"+str(datetime.datetime.now())+".txt","w+")
for scenario in scenarios:
d = analysis[scenario]
......@@ -141,12 +192,25 @@ def analyzeData():
tc = nc + c + uc
f.write ("% of failed contact generation (no candidates found): " + str(nc / tc * 100) + "%\n")
f.write ("% of unstable contact generation (no balanced candidates found): " + str(uc / tc * 100) + "%\n")
f.write ("\n")
if(d["optim_num_trials"] > 0):
f.write ("***********************%\n")
f.write (" optimization related data %\n")
f.write ("% succes rate of optimization: " + str(d["optim_num_success"] / d["optim_num_trials"] * 100) + "%\n")
f.write ("% succes rate of optimization disregarding collision: " + str(d["optim_num_success"] / (d["optim_num_trials"] - d["optim_error_com_proj"]) * 100) + "%\n")
if(d["num_errors"] > 0):
f.write ("% errors due to problem infeasibility (% of errors, % over all trials): " + str(d["optim_error_optim_fail"] / d["num_errors"] * 100) + " " + str(d["optim_error_optim_fail"] / d["optim_num_trials"] * 100) + "%\n")
f.write ("% errors due to unknown reasons (% of errors, % over all trials): " + str(d["optim_error_unknown"] / d["num_errors"] * 100) + " " + str(d["optim_error_unknown"] / d["optim_num_trials"] * 100) + "%\n")
for stat in stats_optim:
printOneStatOptim(f, stat, d)
f.write ("\n \n \n")
f.close()
for scenario in scenarios:
for i in range(0,n_trials):
var = scenario+python_script_extension;
sp.check_call(["./profile.sh", str(var)]);
#~ for i in range(0,n_trials):
#~ var = scenario+python_script_extension;
#~ sp.check_call(["./profile.sh", str(var)]);
#~ time.sleep(3)
parseData(scenario)
analyzeData()
......@@ -2,7 +2,7 @@
gepetto-viewer-server &
hpp-rbprm-server &
ipython ../script/scenarios/$1
ipython ./scenarios/$1
pkill -f 'gepetto-viewer-server'
pkill -f 'hpp-rbprm-server'
#Importing helper class for RBPRM
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
#calling script darpa_hyq_path to compute root path
import darpa_hyq_path as tp
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
rLegId = 'rfleg'
# First joint of the limb, as in urdf file
rLeg = 'rf_haa_joint'
# Last joint of the limb, as in urdf file
rfoot = 'rf_foot_joint'
# Specifying the distance between last joint and contact surface
offset = [0.,-0.021,0.]
# Specifying the contact surface direction when the limb is in rest pose
normal = [0,1,0]
# Specifying the rectangular contact surface length
legx = 0.02; legy = 0.02
# remaining parameters are the chosen heuristic (here, manipulability), and the resolution of the octree (here, 10 cm).
def addLimbDb(limbId, heuristicName, loadValues = True, disableEffectorCollision = False):
fullBody.addLimbDatabase(str(db_dir+limbId+'.db'), limbId, heuristicName,loadValues, disableEffectorCollision)
lLegId = 'lhleg'
rarmId = 'rhleg'
larmId = 'lfleg'
#~ addLimbDb(rLegId, "static")
#~ addLimbDb(lLegId, "static")
#~ addLimbDb(rarmId, "static")
#~ addLimbDb(larmId, "static")
fullBody.addLimb(rLegId,rLeg,rfoot,offset,normal, legx, legy, nbSamples, "jointlimits", 0.1, cType)
lLegId = 'lhleg'
lLeg = 'lh_haa_joint'
lfoot = 'lh_foot_joint'
fullBody.addLimb(lLegId,lLeg,lfoot,offset,normal, legx, legy, nbSamples, "jointlimits", 0.05, cType)
#~
rarmId = 'rhleg'
rarm = 'rh_haa_joint'
rHand = 'rh_foot_joint'
fullBody.addLimb(rarmId,rarm,rHand,offset,normal, legx, legy, nbSamples, "jointlimits", 0.05, cType)
larmId = 'lfleg'
larm = 'lf_haa_joint'
lHand = 'lf_foot_joint'
fullBody.addLimb(larmId,larm,lHand,offset,normal, legx, legy, nbSamples, "jointlimits", 0.05, cType)
q_0 = fullBody.getCurrentConfig();
q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7]
q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = tp.q_goal[0:7]