Unverified Commit 8491161a authored by stonneau's avatar stonneau Committed by GitHub
Browse files

Merge pull request #34 from pFernbach/devel

Devel
parents e4f616ff 4914bb84
......@@ -110,6 +110,10 @@ module hpp
void setPostureWeights(in floatSeq postureWeights)
raises (Error);
/// If true, optimize the orientation of all the newly created contact using a postural task
void usePosturalTaskContactCreation(in boolean use)
raises (Error);
/// set a reference position of the end effector for the given ROM
void setReferenceEndEffector(in string romName, in floatSeq ref)
raises (Error);
......@@ -144,25 +148,32 @@ 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, in unsigned short max_num_sample) 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, in unsigned short max_num_sample) raises (Error);
/// Project a state into a given root position and orientation
///
/// \param stateId target state
/// \param root the root configuration (size 7)
double projectStateToRoot(in unsigned short stateId, in floatSeq root) raises (Error);
/// Project a state into a COM
///
/// \param stateId target state
/// \param com target com
double setConfigAtState(in unsigned short stateId, in floatSeq config) raises (Error);
/// Project a state into a COM
///
/// \param stateId target state
/// \param com target com
double setConfigAtState(in unsigned short stateId, in floatSeq config) 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);
/// 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
......@@ -736,8 +747,10 @@ module hpp
/// \param p 3d position of the point
/// \param n 3d normal of the contact location center
/// \param max_num_sample number of times it will try to randomly sample a configuration before failing
/// \param lockOtherJoints : if True, the values of all the joints exepct the ones of 'limbName' are constrained to be constant.
/// This only affect the first projection, if max_num_sample > 0 and the first projection was unsuccessfull, the parameter is ignored
/// \return (success,NState) whether the creation was successful, as well as the new state
short addNewContact(in unsigned short stateId, in string limbName, in floatSeq position, in floatSeq normal, in unsigned short max_num_sample) raises (Error);
short addNewContact(in unsigned short stateId, in string limbName, in floatSeq position, in floatSeq normal, in unsigned short max_num_sample, in boolean lockOtherJoints) raises (Error);
/// removes a contact from the state
/// if the limb is not already in contact, does nothing
......@@ -779,7 +792,7 @@ module hpp
boolean areKinematicsConstraintsVerifiedForState(in unsigned short stateFrom,in floatSeq point)raises (Error);
floatSeq isReachableFromState(in unsigned short stateFrom, in unsigned short stateTo)raises (Error);
floatSeq isReachableFromState(in unsigned short stateFrom, in unsigned short stateTo, in boolean useIntermediateState)raises (Error);
floatSeq isDynamicallyReachableFromState(in unsigned short stateFrom, in unsigned short stateTo,in boolean addPathPerPhase, in floatSeq timings,in short numPointsPerPhase)raises (Error);
......
from hpp.corbaserver.rbprm.hrp2 import Robot
from hpp.gepetto import Viewer
from tools.display_tools import *
import time
print "Plan guide trajectory ..."
import hrp2_flatGround_path as tp
print "Done."
import time
pId = tp.ps.numberPaths() -1
fullBody = Robot ()
# Set the bounds for the root
fullBody.setJointBounds ("root_joint", [-5,5, -1.5, 1.5, 0.5, 0.8])
fullBody.setJointBounds("LLEG_JOINT3",[0.4,2.61799]) # increase min bounds on knee, required to stay in the 'comfort zone' of the stabilizer
fullBody.setJointBounds("RLEG_JOINT3",[0.4,2.61799])
# TODO : fix rot y legs
# add the 6 extraDof for velocity and acceleration (see *_path.py script)
fullBody.client.robot.setDimensionExtraConfigSpace(tp.extraDof)
fullBody.client.robot.setExtraConfigSpaceBounds([-tp.vMax,tp.vMax,-tp.vMax,tp.vMax,0,0,-tp.aMax,tp.aMax,-tp.aMax,tp.aMax,0,0])
ps = tp.ProblemSolver( fullBody )
ps.setParameter("Kinodynamic/velocityBound",tp.vMax)
ps.setParameter("Kinodynamic/accelerationBound",tp.aMax)
#load the viewer
v = tp.Viewer (ps,viewerClient=tp.v.client, displayCoM = True)
# load a reference configuration
q_ref = fullBody.referenceConfig[::]+[0]*6
q_init = q_ref[::]
fullBody.setReferenceConfig(q_ref)
fullBody.setPostureWeights(fullBody.postureWeights[::]+[0]*6)
fullBody.setCurrentConfig (q_init)
v(q_ref)
print "Generate limb DB ..."
tStart = time.time()
# generate databases :
nbSamples = 100000
fullBody.addLimb(fullBody.rLegId,fullBody.rleg,'',fullBody.rLegOffset,fullBody.rLegNormal, fullBody.rLegx, fullBody.rLegy, nbSamples, "fixedStep06", 0.01,limbOffset=fullBody.rLegLimbOffset,kinematicConstraintsPath=fullBody.rLegKinematicConstraints,kinematicConstraintsMin = 0.4)
fullBody.runLimbSampleAnalysis(fullBody.rLegId, "ReferenceConfiguration", True)
fullBody.addLimb(fullBody.lLegId,fullBody.lleg,'',fullBody.lLegOffset,fullBody.lLegNormal, fullBody.lLegx, fullBody.lLegy, nbSamples, "fixedStep06", 0.01,limbOffset=fullBody.lLegLimbOffset,kinematicConstraintsPath=fullBody.lLegKinematicConstraints,kinematicConstraintsMin = 0.4)
fullBody.runLimbSampleAnalysis(fullBody.lLegId, "ReferenceConfiguration", True)
"""
fullBody.addLimb(fullBody.rArmId,fullBody.rarm,fullBody.rhand,fullBody.rArmOffset,fullBody.rArmNormal, fullBody.rArmx, fullBody.rArmy, nbSamples, "static", 0.01,kinematicConstraintsPath=fullBody.rArmKinematicConstraints)
fullBody.runLimbSampleAnalysis(fullBody.rArmId, "ReferenceConfiguration", True)
fullBody.addLimb(fullBody.lArmId,fullBody.larm,fullBody.lhand,fullBody.lArmOffset,fullBody.lArmNormal, fullBody.lArmx, fullBody.lArmy, nbSamples, "static", 0.01,kinematicConstraintsPath=fullBody.lArmKinematicConstraints)
fullBody.runLimbSampleAnalysis(fullBody.lArmId, "ReferenceConfiguration", True)
"""
tGenerate = time.time() - tStart
print "Done."
print "Databases generated in : "+str(tGenerate)+" s"
#define initial and final configurations :
configSize = fullBody.getConfigSize() -fullBody.client.robot.getDimensionExtraConfigSpace()
q_init[0:7] = tp.ps.configAtParam(pId,0.01)[0:7] # use this to get the correct orientation
q_goal = q_init[::]; q_goal[0:7] = tp.ps.configAtParam(pId,tp.ps.pathLength(pId))[0:7]
dir_init = tp.ps.configAtParam(pId,0.01)[tp.indexECS:tp.indexECS+3]
acc_init = tp.ps.configAtParam(pId,0.01)[tp.indexECS+3:tp.indexECS+6]
dir_goal = tp.ps.configAtParam(pId,tp.ps.pathLength(pId)-0.01)[tp.indexECS:tp.indexECS+3]
acc_goal = [0,0,0]
robTreshold = 3
# copy extraconfig for start and init configurations
q_init[configSize:configSize+3] = dir_init[::]
q_init[configSize+3:configSize+6] = acc_init[::]
q_goal[configSize:configSize+3] = dir_goal[::]
q_goal[configSize+3:configSize+6] = [0,0,0]
q_init[2] = q_ref[2]
q_goal[2] = q_ref[2]
fullBody.setStaticStability(True)
fullBody.setCurrentConfig (q_init)
v(q_init)
fullBody.setCurrentConfig (q_goal)
v(q_goal)
v.addLandmark('hrp2_14/base_link',0.3)
v(q_init)
# specify the full body configurations as start and goal state of the problem
fullBody.setStartState(q_init,[fullBody.rLegId,fullBody.lLegId])
fullBody.setEndState(q_goal,[fullBody.rLegId,fullBody.lLegId])
print "Generate contact plan ..."
tStart = time.time()
configs = fullBody.interpolate(0.01,pathId=pId,robustnessTreshold = 1., filterStates = True,testReachability = False, quasiStatic = False)
tInterpolateConfigs = time.time() - tStart
print "Done."
print "Contact plan generated in : "+str(tInterpolateConfigs)+" s"
print "number of configs :", len(configs)
#raw_input("Press Enter to display the contact sequence ...")
#displayContactSequence(v,configs)
from hpp.corbaserver.rbprm.hrp2_abstract import Robot
from hpp.gepetto import Viewer
from hpp.corbaserver import ProblemSolver
import time
vMax = 0.2# linear velocity bound for the root
aMax = 0.1# linear acceleration bound for the root
extraDof = 6
mu=0.5# coefficient of friction
# Creating an instance of the helper class, and loading the robot
rbprmBuilder = Robot ()
# Define bounds for the root : bounding box of the scenario
rbprmBuilder.setJointBounds ("root_joint", [-5,5, -1.5, 1.5, 0.5, 0.8])
# The following lines set constraint on the valid configurations:
# a configuration is valid only if all limbs can create a contact with the corresponding afforcances type
rbprmBuilder.setFilter([Robot.rLegId,Robot.lLegId])
rbprmBuilder.setAffordanceFilter(Robot.rLegId, ['Support',])
rbprmBuilder.setAffordanceFilter(Robot.lLegId, ['Support'])
# We also bound the rotations of the torso. (z, y, x)
rbprmBuilder.boundSO3([-1.7,1.7,-0.1,0.1,-0.1,0.1])
# Add 6 extraDOF to the problem, used to store the linear velocity and acceleration of the root
rbprmBuilder.client.robot.setDimensionExtraConfigSpace(extraDof)
# We set the bounds of this extraDof with velocity and acceleration bounds (expect on z axis)
rbprmBuilder.client.robot.setExtraConfigSpaceBounds([-vMax,vMax,-vMax,vMax,0,0,-aMax,aMax,-aMax,aMax,0,0])
indexECS = rbprmBuilder.getConfigSize() - rbprmBuilder.client.robot.getDimensionExtraConfigSpace()
# Creating an instance of HPP problem solver
ps = ProblemSolver( rbprmBuilder )
# define parameters used by various methods :
ps.setParameter("Kinodynamic/velocityBound",vMax)
ps.setParameter("Kinodynamic/accelerationBound",aMax)
ps.setParameter("DynamicPlanner/sizeFootX",Robot.legX*2.)
ps.setParameter("DynamicPlanner/sizeFootY",Robot.legY*2.)
ps.setParameter("DynamicPlanner/friction",mu)
# sample only configuration with null velocity and acceleration :
ps.setParameter("ConfigurationShooter/sampleExtraDOF",False)
# initialize the viewer :
from hpp.gepetto import ViewerFactory
vf = ViewerFactory (ps)
# load the module to analyse the environnement and compute the possible contact surfaces
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool ()
afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
afftool.loadObstacleModel ("hpp_environments", "multicontact/ground", "planning", vf)
v = vf.createViewer(displayArrows = True)
#afftool.visualiseAffordances('Support', v, v.color.lightBrown)
# Setting initial configuration
q_init = rbprmBuilder.getCurrentConfig ();
q_init[3:7] = [0,0,0,1]
q_init [0:3] = [0, 0, 0.6]
v (q_init)
ps.setInitialConfig (q_init)
# set goal config
rbprmBuilder.setCurrentConfig (q_init)
q_goal = q_init [::]
q_goal[0] = 1.5
v(q_goal)
ps.addGoalConfig (q_goal)
# Choosing RBPRM shooter and path validation methods.
ps.selectConfigurationShooter("RbprmShooter")
ps.addPathOptimizer ("RandomShortcutDynamic")
ps.selectPathValidation("RbprmPathValidation",0.05)
# Choosing kinodynamic methods :
ps.selectSteeringMethod("RBPRMKinodynamic")
ps.selectDistance("Kinodynamic")
ps.selectPathPlanner("DynamicPlanner")
# Solve the planning problem :
t = ps.solve ()
print "Guide planning time : ",t
# display solution :
from hpp.gepetto import PathPlayer
pp = PathPlayer (v)
pp.dt=0.1
#pp.displayVelocityPath(0)
v.client.gui.setVisibility("path_0_root","ALWAYS_ON_TOP")
pp.dt=0.01
#pp(0)
# move the robot out of the view before computing the contacts
q_far = q_init[::]
q_far[2] = -2
v(q_far)
......@@ -26,16 +26,18 @@ q_ref = fullBody.referenceConfig[::]+[0]*6
q_init = q_ref[::]
fullBody.setReferenceConfig(q_ref)
fullBody.setCurrentConfig (q_init)
fullBody.setPostureWeights(fullBody.postureWeights[::]+[0]*6)
fullBody.usePosturalTaskContactCreation(True)
print "Generate limb DB ..."
tStart = time.time()
# generate databases :
nbSamples = 50000
fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.rLegOffset,fullBody.rLegNormal, fullBody.rLegx, fullBody.rLegy, nbSamples, "fixedStep08", 0.01,kinematicConstraintsPath=fullBody.rLegKinematicConstraints)
fullBody.runLimbSampleAnalysis(fullBody.rLegId, "ReferenceConfiguration", True)
fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.lLegOffset,fullBody.rLegNormal, fullBody.lLegx, fullBody.lLegy, nbSamples, "fixedStep08", 0.01,kinematicConstraintsPath=fullBody.lLegKinematicConstraints)
fullBody.runLimbSampleAnalysis(fullBody.lLegId, "ReferenceConfiguration", True)
fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.rLegOffset,fullBody.rLegNormal, fullBody.rLegx, fullBody.rLegy, nbSamples, "fixedStep06", 0.01,kinematicConstraintsPath=fullBody.rLegKinematicConstraints,kinematicConstraintsMin = 0.75)
#fullBody.runLimbSampleAnalysis(fullBody.rLegId, "ReferenceConfiguration", True)
fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.lLegOffset,fullBody.rLegNormal, fullBody.lLegx, fullBody.lLegy, nbSamples, "fixedStep06", 0.01,kinematicConstraintsPath=fullBody.lLegKinematicConstraints,kinematicConstraintsMin = 0.75)
#fullBody.runLimbSampleAnalysis(fullBody.lLegId, "ReferenceConfiguration", True)
tGenerate = time.time() - tStart
......@@ -45,18 +47,18 @@ print "Databases generated in : "+str(tGenerate)+" s"
#define initial and final configurations :
configSize = fullBody.getConfigSize() -fullBody.client.robot.getDimensionExtraConfigSpace()
q_init[0:7] = tp.ps.configAtParam(pId,0.01)[0:7] # use this to get the correct orientation
q_init[0:7] = tp.ps.configAtParam(pId,0.001)[0:7] # use this to get the correct orientation
q_goal = q_init[::]; q_goal[0:7] = tp.ps.configAtParam(pId,tp.ps.pathLength(pId))[0:7]
dir_init = tp.ps.configAtParam(pId,0.01)[tp.indexECS:tp.indexECS+3]
acc_init = tp.ps.configAtParam(pId,0.01)[tp.indexECS+3:tp.indexECS+6]
dir_goal = tp.ps.configAtParam(pId,tp.ps.pathLength(pId)-0.01)[tp.indexECS:tp.indexECS+3]
vel_init = tp.ps.configAtParam(pId,0)[tp.indexECS:tp.indexECS+3]
acc_init = tp.ps.configAtParam(pId,0)[tp.indexECS+3:tp.indexECS+6]
vel_goal = tp.ps.configAtParam(pId,tp.ps.pathLength(pId))[tp.indexECS:tp.indexECS+3]
acc_goal = [0,0,0]
robTreshold = 3
# copy extraconfig for start and init configurations
q_init[configSize:configSize+3] = dir_init[::]
q_init[configSize:configSize+3] = vel_init[::]
q_init[configSize+3:configSize+6] = acc_init[::]
q_goal[configSize:configSize+3] = dir_goal[::]
q_goal[configSize:configSize+3] = vel_goal[::]
q_goal[configSize+3:configSize+6] = [0,0,0]
......@@ -81,7 +83,7 @@ fullBody.setEndState(q_goal,[fullBody.rLegId,fullBody.lLegId])
print "Generate contact plan ..."
tStart = time.time()
configs = fullBody.interpolate(0.01,pathId=pId,robustnessTreshold = 2, filterStates = True)
configs = fullBody.interpolate(0.01,pathId=pId,robustnessTreshold = 2, filterStates = True,quasiStatic=True)
tInterpolateConfigs = time.time() - tStart
print "Done."
print "Contact plan generated in : "+str(tInterpolateConfigs)+" s"
......
......@@ -6,7 +6,7 @@ print "Plan guide trajectory ..."
import talos_navBauzil_path as tp
print "Done."
import time
Robot.urdfSuffix += "_safeFeet"
pId = tp.ps.numberPaths() -1
......@@ -34,17 +34,18 @@ q_ref = fullBody.referenceConfig[::]+[0,0,0,0,0,0]
q_init = q_ref[::]
fullBody.setReferenceConfig(q_ref)
fullBody.setCurrentConfig (q_init)
fullBody.setPostureWeights(fullBody.postureWeights[::]+[0]*6)
fullBody.usePosturalTaskContactCreation(True)
print "Generate limb DB ..."
tStart = time.time()
# generate databases :
nbSamples = 10000
fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.rLegOffset,fullBody.rLegNormal, fullBody.rLegx, fullBody.rLegy, nbSamples, "fixedStep08", 0.01)
fullBody.runLimbSampleAnalysis(fullBody.rLegId, "ReferenceConfiguration", True)
fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.rLegOffset,fullBody.rLegNormal, fullBody.rLegx, fullBody.rLegy, nbSamples, "fixedStep06", 0.01,kinematicConstraintsPath=fullBody.rLegKinematicConstraints,kinematicConstraintsMin = 0.85)
#fullBody.runLimbSampleAnalysis(fullBody.rLegId, "ReferenceConfiguration", True)
#fullBody.saveLimbDatabase(rLegId, "./db/talos_rLeg_walk.db")
fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.lLegOffset,fullBody.rLegNormal, fullBody.lLegx, fullBody.lLegy, nbSamples, "fixedStep08", 0.01)
fullBody.runLimbSampleAnalysis(fullBody.lLegId, "ReferenceConfiguration", True)
fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.lLegOffset,fullBody.rLegNormal, fullBody.lLegx, fullBody.lLegy, nbSamples, "fixedStep06", 0.01,kinematicConstraintsPath=fullBody.lLegKinematicConstraints,kinematicConstraintsMin = 0.85)
#fullBody.runLimbSampleAnalysis(fullBody.lLegId, "ReferenceConfiguration", True)
#fullBody.saveLimbDatabase(rLegId, "./db/talos_lLeg_walk.db")
tGenerate = time.time() - tStart
......@@ -89,7 +90,7 @@ fullBody.setEndState(q_goal,[fullBody.lLegId,fullBody.rLegId])
print "Generate contact plan ..."
tStart = time.time()
configs = fullBody.interpolate(0.01,pathId=pId,robustnessTreshold = 2, filterStates = True,testReachability=False)
configs = fullBody.interpolate(0.01,pathId=pId,robustnessTreshold = 2, filterStates = True,testReachability=True,quasiStatic=True)
tInterpolateConfigs = time.time() - tStart
print "Done."
print "number of configs :", len(configs)
......
......@@ -6,7 +6,7 @@ print "Plan guide trajectory ..."
import talos_navBauzil_hard_path as tp
print "Done."
import time
Robot.urdfSuffix += "_safeFeet"
pId = tp.ps.numberPaths() -1
fullBody = Robot ()
......@@ -30,20 +30,20 @@ ps.setParameter("Kinodynamic/accelerationBound",tp.aMax)
v = tp.Viewer (ps,viewerClient=tp.v.client, displayCoM = True)
# load a reference configuration
q_ref = fullBody.referenceConfig_legsApart[::] + [0]*6
q_ref = fullBody.referenceConfig[::] + [0]*6
q_init = q_ref[::]
fullBody.setReferenceConfig(q_ref)
fullBody.setCurrentConfig (q_init)
fullBody.setPostureWeights(fullBody.postureWeights[::]+[0]*6)
fullBody.usePosturalTaskContactCreation(True)
print "Generate limb DB ..."
tStart = time.time()
# generate databases :
nbSamples = 20000
fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.rLegOffset,fullBody.rLegNormal, fullBody.rLegx, fullBody.rLegy, nbSamples, "fixedStep06", 0.01)
fullBody.runLimbSampleAnalysis(fullBody.rLegId, "ReferenceConfiguration", True)
fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.lLegOffset,fullBody.rLegNormal, fullBody.lLegx, fullBody.lLegy, nbSamples, "fixedStep06", 0.01)
fullBody.runLimbSampleAnalysis(fullBody.lLegId, "ReferenceConfiguration", True)
nbSamples = 10000
fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.rLegOffset,fullBody.rLegNormal, fullBody.rLegx, fullBody.rLegy, nbSamples, "fixedStep06", 0.01,kinematicConstraintsPath=fullBody.rLegKinematicConstraints,kinematicConstraintsMin = 0.85)
fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.lLegOffset,fullBody.rLegNormal, fullBody.lLegx, fullBody.lLegy, nbSamples, "fixedStep06", 0.01,kinematicConstraintsPath=fullBody.lLegKinematicConstraints,kinematicConstraintsMin = 0.85)
#In this scenario, the arms are not used to create contact, but they may move to avoid collision.
fullBody.addNonContactingLimb(fullBody.lArmId,fullBody.larm,fullBody.lhand,5000)
......@@ -93,7 +93,7 @@ fullBody.setEndState(q_goal,[fullBody.lLegId,fullBody.rLegId])
print "Generate contact plan ..."
tStart = time.time()
configs = fullBody.interpolate(0.01,pathId=pId,robustnessTreshold = 2, filterStates = True)
configs = fullBody.interpolate(0.01,pathId=pId,robustnessTreshold = 2, filterStates = True,testReachability=True,quasiStatic=True)
tInterpolateConfigs = time.time() - tStart
print "Done."
print "number of configs :", len(configs)
......
......@@ -4,7 +4,7 @@ from hpp.corbaserver import Client
from hpp.corbaserver import ProblemSolver
import time
Robot.urdfName += "_large"
vMax = 0.3 # linear velocity bound for the root
aMax = 0.1 # linear acceleration bound for the root
extraDof = 6
......@@ -61,6 +61,7 @@ v.addLandmark(v.sceneName,1)
# Setting initial configuration
q_init = rbprmBuilder.getCurrentConfig ();
q_init[8] = 0.006761 # torso 2 position in reference config
q_init [0:3] = [-0.7,2,0.98]
v (q_init)
ps.setInitialConfig (q_init)
......
......@@ -61,6 +61,7 @@ v.addLandmark(v.sceneName,1)
# Setting initial configuration
q_init = rbprmBuilder.getCurrentConfig ();
q_init[8] = 0.006761 # torso 2 position in reference config
q_init [0:3] = [-0.9,1.5,0.98]
q_init[-6:-3] = [0.07,0,0]
v (q_init)
......
from hpp.corbaserver.rbprm.talos import Robot
from hpp.gepetto import Viewer
from tools.display_tools import *
import time
print "Plan guide trajectory ..."
import talos_plateformes_path as tp
print "Done."
import time
pId = tp.ps.numberPaths() -1
fullBody = Robot ()
# Set the bounds for the root
fullBody.setJointBounds ("root_joint", [-5,5, -1.5, 1.5, 0.95, 1.3])
fullBody.setConstrainedJointsBounds()
fullBody.setJointBounds('leg_left_1_joint',[-0.1,0.2])
fullBody.setJointBounds('leg_right_1_joint',[-0.2,0.1])
# add the 6 extraDof for velocity and acceleration (see *_path.py script)
fullBody.client.robot.setDimensionExtraConfigSpace(tp.extraDof)
fullBody.client.robot.setExtraConfigSpaceBounds([-tp.vMax,tp.vMax,-tp.vMax,tp.vMax,0,0,-tp.aMax,tp.aMax,-tp.aMax,tp.aMax,0,0])
ps = tp.ProblemSolver( fullBody )
ps.setParameter("Kinodynamic/velocityBound",tp.vMax)
ps.setParameter("Kinodynamic/accelerationBound",tp.aMax)
#load the viewer
v = tp.Viewer (ps,viewerClient=tp.v.client, displayCoM = True)
# load a reference configuration
q_ref = fullBody.referenceConfig[::]+[0]*6
q_init = q_ref[::]
fullBody.setReferenceConfig(q_ref)
fullBody.setCurrentConfig (q_init)
fullBody.setPostureWeights(fullBody.postureWeights[::]+[0]*6)
#fullBody.usePosturalTaskContactCreation(True)
print "Generate limb DB ..."
tStart = time.time()
# generate databases :
nbSamples = 100000
fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.rLegOffset,fullBody.rLegNormal, fullBody.rLegx, fullBody.rLegy, nbSamples, "fixedStep08", 0.01,kinematicConstraintsPath=fullBody.rLegKinematicConstraints,kinematicConstraintsMin = 0.6)
fullBody.runLimbSampleAnalysis(fullBody.rLegId, "ReferenceConfiguration", True)
fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.lLegOffset,fullBody.rLegNormal, fullBody.lLegx, fullBody.lLegy, nbSamples, "fixedStep08", 0.01,kinematicConstraintsPath=fullBody.lLegKinematicConstraints,kinematicConstraintsMin = 0.6)
fullBody.runLimbSampleAnalysis(fullBody.lLegId, "ReferenceConfiguration", True)
tGenerate = time.time() - tStart
print "Done."
print "Databases generated in : "+str(tGenerate)+" s"
#define initial and final configurations :
configSize = fullBody.getConfigSize() -fullBody.client.robot.getDimensionExtraConfigSpace()
q_init[0:7] = tp.ps.configAtParam(pId,0.)[0:7] # use this to get the correct orientation
q_goal = q_init[::]; q_goal[0:7] = tp.ps.configAtParam(pId,tp.ps.pathLength(pId))[0:7]
vel_init = tp.ps.configAtParam(pId,0)[tp.indexECS:tp.indexECS+3]
acc_init = tp.ps.configAtParam(pId,0)[tp.indexECS+3:tp.indexECS+6]
vel_goal = tp.ps.configAtParam(pId,tp.ps.pathLength(pId))[tp.indexECS:tp.indexECS+3]
acc_goal = [0,0,0]
robTreshold = 1
# copy extraconfig for start and init configurations
q_init[configSize:configSize+3] = vel_init[::]
q_init[configSize+3:configSize+6] = acc_init[::]
q_goal[configSize:configSize+3] = vel_goal[::]
q_goal[configSize+3:configSize+6] = [0,0,0]
q_init[2] = q_ref[2]+0.16
q_goal[2] = q_ref[2]+0.16
fullBody.setStaticStability(True)
fullBody.setCurrentConfig (q_init)
v(q_init)
fullBody.setCurrentConfig (q_goal)
v(q_goal)
v.addLandmark('talos/base_link',0.3)
v(q_init)
# specify the full body configurations as start and goal state of the problem
fullBody.setStartState(q_init,[fullBody.rLegId,fullBody.lLegId])
fullBody.setEndState(q_goal,[fullBody.rLegId,fullBody.lLegId])
print "Generate contact plan ..."
tStart = time.time()
configs = fullBody.interpolate(0.01,pathId=pId,robustnessTreshold = robTreshold, filterStates = True,quasiStatic=True)
tInterpolateConfigs = time.time() - tStart
print "Done."
print "Contact plan generated in : "+str(tInterpolateConfigs)+" s"
print "number of configs :", len(configs)
#raw_input("Press Enter to display the contact sequence ...")
#displayContactSequence(v,configs)
fullBody.resetJointsBounds()
from hpp.corbaserver.rbprm.talos_abstract import Robot
from hpp.gepetto import Viewer
from hpp.corbaserver import ProblemSolver
import time
vMax = 0.3# linear velocity bound for the root
aMax = 0.1# linear acceleration bound for the root
extraDof = 6
mu=0.5# coefficient of friction
# Creating an instance of the helper class, and loading the robot
# Creating an instance of the helper class, and loading the robot
rbprmBuilder = Robot ()