Commit 85b2ae88 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

Merge tag 'v4.7.0'

Release of version 4.7.0.
parents bcfdd50e 5173611a
Pipeline #6094 passed with stage
in 19 minutes and 47 seconds
......@@ -22,8 +22,8 @@ SET(CXX_DISABLE_WERROR true)
INCLUDE(cmake/base.cmake)
INCLUDE(cmake/idl.cmake)
INCLUDE(cmake/python.cmake)
INCLUDE(cmake/hpp.cmake)
INCLUDE(cmake/python.cmake)
SET(PROJECT_NAME hpp-rbprm-corba)
SET(PROJECT_DESCRIPTION "Corba server for reachability based planning")
......@@ -35,15 +35,13 @@ SET(CUSTOM_HEADER_DIR hpp/corbaserver/rbprm)
SETUP_HPP_PROJECT ()
LIST(APPEND PKG_CONFIG_ADDITIONAL_VARIABLES cmake_plugin)
SET(${PROJECT_NAME}_HEADERS
include/hpp/corbaserver/rbprm/server.hh
)
# Activate hpp-util logging if requested
SET (HPP_DEBUG FALSE CACHE BOOL "trigger hpp-util debug output")
IF (HPP_DEBUG)
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DHPP_DEBUG")
ENDIF()
FINDPYTHON()
ADD_DOC_DEPENDENCY("hpp-core >= 4.3")
ADD_REQUIRED_DEPENDENCY("hpp-corbaserver >= 4.3")
......
# Humanoid Path Planner - RBPRM-CORBA module
[![Pipeline status](https://gepgitlab.laas.fr/humanoid-path-planner/hpp-rbprm-corba/badges/master/pipeline.svg)](https://gepgitlab.laas.fr/humanoid-path-planner/hpp-rbprm-corba/commits/master)
[![Coverage report](https://gepgitlab.laas.fr/humanoid-path-planner/hpp-rbprm-corba/badges/master/coverage.svg?job=doc-coverage)](http://projects.laas.fr/gepetto/doc/humanoid-path-planner/hpp-rbprm-corba/master/coverage/)
Copyright 2015 LAAS-CNRS
Author: Steve Tonneau
......@@ -63,9 +66,9 @@ If you are planning to use the visualization tools used by the Gepetto team, alo
`wget https://raw.githubusercontent.com/humanoid-path-planner/hpp-rbprm-corba/master/script/scenarios/demos/darpa_hyq.py`
`wget https://raw.githubusercontent.com/humanoid-path-planner/hpp-rbprm-corba/master/script/scenarios/demos/darpa_hyq_path.py`
`wget https://raw.githubusercontent.com/humanoid-path-planner/hpp-rbprm-corba/devel/script/scenarios/demos/run.sh`
- Make the run.sh script executable:`chmod +x run.sh`
......
Subproject commit 61e5574a0615706aab06986f6aecf665ddc31141
Subproject commit c81a37191d764522899b5dcb4468485a826141c2
......@@ -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,39 @@ 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);
/// Clone a state
///
/// \param stateId target state
/// \return stateId of the cloned state
short cloneState(in unsigned short stateId) 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 given root position and orientation
///
/// \param stateId target state
/// \param root the root configuration (size 7)
/// \param offset specific point to be projected in root frame. If different than 0 root orientation is ignored
double projectStateToRoot(in unsigned short stateId, in floatSeq root, in floatSeq offset) 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);
/// 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);
/// Get Sample configuration by its id
/// \param sampleName name of the limb from which to retrieve a sample
......@@ -244,6 +262,13 @@ module hpp
floatSeqSeq getContactSamplesProjected(in string name, in floatSeq dofArray, in floatSeq direction,
in unsigned short numSamples) raises (Error);
/// Given a contact state and a limb, tries to generate a new contact, and returns the id of the new State if successful
/// \param currentState Id of the considered state
/// \param name name of the limb used to create a contact
/// \param direction desired direction of motion for the robot
/// \return id of the new contact state if successful, -1 otherwise
short generateContactState(in unsigned short currentState, in string name, in floatSeq direction) raises (Error);
/// get Ids of limb in an octree cell
/// \param name name of the considered limb
/// \param octreeNodeId considered configuration of the robot
......@@ -353,7 +378,7 @@ module hpp
/// \param filterStates If different than 0, the resulting state list will be filtered to remove unnecessary states
/// \param testReachability : if true, check each contact transition with our reachability criterion
/// \param quasiStatic : if True, use our reachability criterion with the quasiStatic constraint
floatSeqSeq interpolate(in double timestep, in double path, in double robustnessTreshold, in unsigned short filterStates, in boolean testReachability, in boolean quasiStatic) raises (Error);
floatSeqSeq interpolate(in double timestep, in double path, in double robustnessTreshold, in unsigned short filterStates, in boolean testReachability, in boolean quasiStatic, in boolean erasePreviousStates) 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
......@@ -364,7 +389,7 @@ module hpp
/// \param filterStates If different than 0, the resulting state list will be filtered to remove unnecessary states
/// \param testReachability : if true, check each contact transition with our reachability criterion
/// \param quasiStatic : if True, use our reachability criterion with the quasiStatic constraint
floatSeqSeq interpolateConfigs(in floatSeqSeq configs, in double robustnessTreshold, in unsigned short filterStates,in boolean testReachability, in boolean quasiStatic) raises (Error);
floatSeqSeq interpolateConfigs(in floatSeqSeq configs, in double robustnessTreshold, in unsigned short filterStates,in boolean testReachability, in boolean quasiStatic, in boolean erasePreviousStates) raises (Error);
/// returns the CWC of the robot at a given state
......@@ -652,6 +677,10 @@ module hpp
/// \return whether the limb is in contact at this state
short computeIntermediary(in unsigned short state1, in unsigned short state2) raises (Error);
/// Compute the number of computed states
/// \return the number of computed states
short getNumStates() 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.
/// \param filename name of the file used to save the contacts.
......@@ -725,6 +754,17 @@ module hpp
/// \param stateIdTo : index of the second state
Names_t getContactsVariations(in unsigned short stateIdFrom,in unsigned short stateIdTo) raises (Error);
/// For a given limb, returns the names of all the contact surfaces in collisions with the limb's reachable workspace
/// \param configuration : root configuration
/// \param limbName : name of the considered limb
Names_t getCollidingObstacleAtConfig(in floatSeq configuration,in string limbName) raises (Error);
/// For a given limb, return all the intersections between the limb reachable workspace and a contact surface
/// \param configuration : root configuration
/// \param limbName : name of the considered limb
floatSeqSeq getContactSurfacesAtConfig(in floatSeq configuration,in string limbName) raises (Error);
/// return a list of all the limbs names
Names_t getAllLimbsNames() raises (Error);
/// tries to add a new contact to the state
......@@ -736,8 +776,11 @@ 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
/// \param rotation : desired rotation of the end-effector in contact, expressed as Quaternion (x,y,z,w). If different from zero, the normal is ignored. Otherwise the rotation is automatically computed from the normal (with one axis of freedom)
/// \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,in floatSeq rotation) raises (Error);
/// removes a contact from the state
/// if the limb is not already in contact, does nothing
......@@ -775,11 +818,13 @@ module hpp
floatSeqSeq getPathAsBezier(in unsigned short pathId)raises (Error);
boolean toggleNonContactingLimb(in string limbname)raises (Error);
boolean areKinematicsConstraintsVerified(in floatSeq point)raises (Error);
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);
......
......@@ -21,29 +21,30 @@
# define HPP_RBPRM_CORBA_SERVER_HH
# include <hpp/corba/template/server.hh>
# include <hpp/corbaserver/rbprm/config.hh>
# include <hpp/corbaserver/problem-solver-map.hh>
# include <hpp/corbaserver/rbprm/config.hh>
# include <hpp/corbaserver/server-plugin.hh>
namespace hpp {
namespace rbprm {
namespace impl {
class RbprmBuilder;
}
class HPP_RBPRM_CORBA_DLLAPI Server
class HPP_RBPRM_CORBA_DLLAPI Server : public corbaServer::ServerPlugin
{
public:
Server (int argc, const char *argv[], bool multiThread = false,
const std::string& poaName = "child");
Server (corbaServer::Server* parent);
~Server ();
/// Set planner that will be controlled by server
void setProblemSolverMap (hpp::corbaServer::ProblemSolverMapPtr_t psMap);
/// Start corba server
/// Call hpp::corba::Server <impl::Problem>::startCorbaServer
void startCorbaServer(const std::string& contextId,
const std::string& contextKind,
const std::string& objectId);
const std::string& contextKind);
std::string name () const;
public:
corba::Server <impl::RbprmBuilder>* rbprmBuilder_;
}; // class Server
......
......@@ -103,7 +103,7 @@ def genPlan(stepsize=0.06):
start = time.time()
configs = fullBody.interpolate(stepsize, 5, 0., filterStates = True,testReachability = False)
end = time.time()
print "Contact plan generated in " + str(end-start) + "seconds"
print("Contact plan generated in " + str(end-start) + "seconds")
def contactPlan(step = 0.5):
v.client.gui.setVisibility("hyq", "ON")
......@@ -115,6 +115,6 @@ def contactPlan(step = 0.5):
time.sleep(step)
print "Root path generated in " + str(tp.t) + " ms."
print("Root path generated in " + str(tp.t) + " ms.")
genPlan(0.01);contactPlan(0.01)
#Importing helper class for RBPRM
from hpp.corbaserver.rbprm.hyq import Robot
from hpp.corbaserver.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_HPP_DIR']
db_dir = ins_dir+"/install/share/hyq-rbprm/database/hyq_"
from hpp.corbaserver import Client
# This time we load the full body model of HyQ
fullBody = Robot ()
fullBody.setJointBounds ("root_joint", [-2,5, -1, 1, 0.3, 4])
fullBody.client.robot.setDimensionExtraConfigSpace(6)
fullBody.client.robot.setExtraConfigSpaceBounds([0,0,0,0,0,0,0,0,0,0,0,0])
# Setting a number of sample configurations used
nbSamples = 10000
ps = tp.ProblemSolver(fullBody)
v = tp.Viewer (ps, viewerClient=tp.v.client)
rootName = 'base_joint_xyz'
cType = "_3_DOF"
def addLimbDb(limbId, heuristicName, loadValues = True, disableEffectorCollision = False):
fullBody.addLimbDatabase(str(db_dir+limbId+'.db'), limbId, heuristicName,loadValues, disableEffectorCollision)
fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "forward", 0.1, cType)
fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "forward", 0.05, cType)
fullBody.addLimb(fullBody.rArmId,fullBody.rarm,fullBody.rhand,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "forward", 0.05, cType)
fullBody.addLimb(fullBody.lArmId,fullBody.larm,fullBody.lhand,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "random", 0.05, cType)
#~ fullBody.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True)
#~ fullBody.runLimbSampleAnalysis(lLegId, "jointLimitsDistance", True)
#~ fullBody.runLimbSampleAnalysis(rarmId, "jointLimitsDistance", True)
#~ fullBody.runLimbSampleAnalysis(larmId, "jointLimitsDistance", True)
q_init=fullBody.referenceConfig[::] + [0]*6; q_init[0:7] = tp.q_init[0:7];
#q_init[2]=fullBody.referenceConfig[2]
q_goal = fullBody.referenceConfig[::]+ [0]*6; q_goal[0:7] = tp.q_goal[0:7];
#q_goal[2]=fullBody.referenceConfig[2]
q_init = fullBody.generateContacts(q_init, [0,0,1])
q_goal = fullBody.generateContacts(q_goal, [0,0,1])
# specifying the full body configurations as start and goal state of the problem
fullBody.setStartState(q_init,[fullBody.rLegId,fullBody.lArmId,fullBody.lLegId,fullBody.rArmId], [[0.,0.,1.] for _ in range(4)])
fullBody.setEndState(q_goal,[fullBody.rLegId,fullBody.lArmId,fullBody.lLegId,fullBody.rArmId], [[0.,0.,1.] for _ in range(4)])
v(q_init)
configs = []
from hpp.gepetto import PathPlayer
pp = PathPlayer (fullBody.client, v)
import time
#DEMO METHODS
def initConfig():
v.client.gui.setVisibility("hyq", "ON")
tp.cl.problem.selectProblem("default")
tp.v.client.gui.setVisibility("toto", "OFF")
tp.v.client.gui.setVisibility("hyq_trunk_large", "OFF")
v(q_init)
def endConfig():
v.client.gui.setVisibility("hyq", "ON")
tp.cl.problem.selectProblem("default")
tp.v.client.gui.setVisibility("toto", "OFF")
tp.v.client.gui.setVisibility("hyq_trunk_large", "OFF")
v(q_goal)
def rootPath():
v.client.gui.setVisibility("hyq", "OFF")
tp.cl.problem.selectProblem("rbprm_path")
tp.v.client.gui.setVisibility("toto", "OFF")
v.client.gui.setVisibility("hyq", "OFF")
tp.v.client.gui.setVisibility("hyq_trunk_large", "ON")
tp.pp(0)
tp.v.client.gui.setVisibility("hyq_trunk_large", "OFF")
v.client.gui.setVisibility("hyq", "ON")
tp.cl.problem.selectProblem("default")
def genPlan(stepsize=0.06):
tp.cl.problem.selectProblem("default")
v.client.gui.setVisibility("hyq", "ON")
tp.v.client.gui.setVisibility("toto", "OFF")
tp.v.client.gui.setVisibility("hyq_trunk_large", "OFF")
global configs
start = time.time()
configs = fullBody.interpolate(stepsize, 5, 0., filterStates = True,testReachability = False)
end = time.time()
print "Contact plan generated in " + str(end-start) + "seconds"
def contactPlan(step = 0.5):
v.client.gui.setVisibility("hyq", "ON")
tp.cl.problem.selectProblem("default")
tp.v.client.gui.setVisibility("toto", "OFF")
tp.v.client.gui.setVisibility("hyq_trunk_large", "OFF")
for i in range(0,len(configs)):
v(configs[i]);
time.sleep(step)
print "Root path generated in " + str(tp.t) + " ms."
genPlan(0.01);contactPlan(0.01)
......@@ -57,7 +57,7 @@ t = ps.solve ()
if isinstance(t, list):
t = t[0]* 3600000 + t[1] * 60000 + t[2] * 1000 + t[3]
print "computation time for root path ", t
print("computation time for root path ", t)
# Playing the computed path
from hpp.gepetto import PathPlayer
......
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)