Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • pfernbac/hpp-rbprm-corba
  • jchemin/hpp-rbprm-corba
  • gsaurel/hpp-rbprm-corba
3 results
Show changes
Commits on Source (276)
176 additional commits have been omitted to prevent performance issues.
Hide whitespace changes
Inline Side-by-side
Showing
with 1403 additions and 169 deletions
.gitlab-ci.yml 0 → 100644
View file @ 3a39a6b3
include: http://rainboard.laas.fr/project/hpp-rbprm-corba/.gitlab-ci.yml
.yapfignore 0 → 100644
View file @ 3a39a6b3
script
profile
affordance-tests
CMakeLists.txt
View file @ 3a39a6b3
# Copyright (c) 2012 CNRS
# Author: Florent Lamiraux
# Copyright (c) 2012, 2020, CNRS
# Authors: Florent Lamiraux, Guilhem Saurel
#
# This file is part of hpp-rbprm-corba.
# hpp-rbprm-corba is free software: you can redistribute it
......@@ -15,49 +15,36 @@
# hpp-rbprm-corba. If not, see
# <http://www.gnu.org/licenses/>.
# Requires at least CMake 2.6 to configure the package.
CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
CMAKE_MINIMUM_REQUIRED(VERSION 3.1)
SET(PROJECT_NAME hpp-rbprm-corba)
SET(PROJECT_DESCRIPTION "Corba server for reachability based planning")
SET(CUSTOM_HEADER_DIR hpp/corbaserver/rbprm)
SET(PROJECT_USE_CMAKE_EXPORT TRUE)
SET(CXX_DISABLE_WERROR true)
INCLUDE(cmake/base.cmake)
INCLUDE(cmake/hpp.cmake)
INCLUDE(cmake/idl.cmake)
INCLUDE(cmake/python.cmake)
INCLUDE(cmake/hpp.cmake)
SET(PROJECT_NAME hpp-rbprm-corba)
SET(PROJECT_DESCRIPTION "Corba server for reachability based planning")
COMPUTE_PROJECT_ARGS(PROJECT_ARGS LANGUAGES CXX)
PROJECT(${PROJECT_NAME} ${PROJECT_ARGS})
# Set to 1 for profiling
#add_definitions(-DPROFILE)
LIST(APPEND PKG_CONFIG_ADDITIONAL_VARIABLES cmake_plugin)
SET(CUSTOM_HEADER_DIR hpp/corbaserver/rbprm)
FINDPYTHON()
SETUP_HPP_PROJECT ()
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()
ADD_DOC_DEPENDENCY("hpp-core >= 4.3")
ADD_REQUIRED_DEPENDENCY("hpp-corbaserver >= 4.3")
ADD_REQUIRED_DEPENDENCY("hpp-rbprm >= 4.3")
ADD_PROJECT_DEPENDENCY("hpp-rbprm" REQUIRED)
ADD_PROJECT_DEPENDENCY("hpp-affordance-corba" REQUIRED)
ADD_PROJECT_DEPENDENCY("hpp-gepetto-viewer")
ADD_REQUIRED_DEPENDENCY("omniORB4 >= 4.1.4")
ADD_REQUIRED_DEPENDENCY("hpp-affordance-corba")
ADD_REQUIRED_DEPENDENCY("hpp-util >= 3")
ADD_REQUIRED_DEPENDENCY("hpp-pinocchio >= 4.3")
ADD_REQUIRED_DEPENDENCY("octomap >= 1.8")
set(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake/find-external/CDD")
find_package(CDD REQUIRED)
ADD_PROJECT_DEPENDENCY(CDD REQUIRED)
add_required_dependency("octomap >= 1.8")
if (OCTOMAP_INCLUDE_DIRS AND OCTOMAP_LIBRARY_DIRS)
if(OCTOMAP_INCLUDE_DIRS AND OCTOMAP_LIBRARY_DIRS)
include_directories(${OCTOMAP_INCLUDE_DIRS})
link_directories(${OCTOMAP_LIBRARY_DIRS})
string(REPLACE "." ";" VERSION_LIST ${OCTOMAP_VERSION})
......@@ -72,9 +59,24 @@ else()
message(STATUS "FCL does not use Octomap")
endif()
PKG_CONFIG_APPEND_LIBS(${PROJECT_NAME})
SET(${PROJECT_NAME}_HEADERS
include/${CUSTOM_HEADER_DIR}/server.hh
)
SET(${PROJECT_NAME}_SOURCES
src/${PROJECT_NAME}.cc
)
# Stand alone corba server
ADD_EXECUTABLE(hpp-rbprm-server ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS})
TARGET_INCLUDE_DIRECTORIES(hpp-rbprm-server PUBLIC $<INSTALL_INTERFACE:include>)
TARGET_LINK_LIBRARIES(hpp-rbprm-server hpp-corbaserver::hpp-corbaserver)
INSTALL(TARGETS hpp-rbprm-server EXPORT ${TARGETS_EXPORT_NAME} DESTINATION bin)
ADD_SUBDIRECTORY(src)
ADD_SUBDIRECTORY(tests)
CONFIG_FILES (include/hpp/corbaserver/rbprm/doc.hh)
CONFIG_FILES (include/${CUSTOM_HEADER_DIR}/doc.hh)
PKG_CONFIG_APPEND_LIBS(${PROJECT_NAME})
SETUP_HPP_PROJECT_FINALIZE()
INSTALL(FILES package.xml DESTINATION share/${PROJECT_NAME})
README.md
View file @ 3a39a6b3
# Humanoid Path Planner - RBPRM-CORBA module
Copyright 2015 LAAS-CNRS
[![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/)
Author: Steve Tonneau
Copyright 2015-2020 LAAS-CNRS
Authors: Steve Tonneau, Pierre Fernbach
## Description
hpp-rbprm-corba implements python bindings for hpp-rbprm, and presents a few example files.
......@@ -10,15 +13,15 @@ Please refer to this [link](https://github.com/humanoid-path-planner/hpp-rbprm)
## Installation from binary package repository
1. Add robotpkg to your apt configuration: http://robotpkg.openrobots.org/debian.html
2. `sudo apt update && sudo apt install robotpkg-hpp-rbprm-corba`
1. Add robotpkg/wip to your apt configuration: http://robotpkg.openrobots.org/robotpkg-wip.html
2. `sudo apt update && sudo apt install robotpkg-pyXX-hpp-rbprm-corba` (replace pyXX with your python version)
3. Then, you will need to export some variables to allow you system to find the executables:
`export PATH=${PATH:+$PATH:}/opt/openrobots/bin:/opt/openrobots/sbin`
`export MANPATH=${MANPATH:+$MANPATH:}/opt/openrobots/man`
`export PYTHONPATH=/opt/openrobots/lib/python2.7/site-packages:$PYTHONPATH`
`export PYTHONPATH=/opt/openrobots/lib/pythonXX/site-packages:$PYTHONPATH`(replace XX with your python version)
`export ROS_PACKAGE_PATH="$ROS_PACKAGE_PATH:/opt/openrobots/share"`
......@@ -35,15 +38,15 @@ Please refer to this [link](https://github.com/humanoid-path-planner/hpp-rbprm)
If you are planning to use the visualization tools used by the Gepetto team, along with python examples, you may need a few extra steps:
1. Install the gepetto-viwer server
1. Install the gepetto-viewer server
`sudo apt install -qqy robotpkg-py27-qt4-gepetto-viewer-corba`
`sudo apt install -qqy robotpkg-pyXX-qt4-gepetto-viewer-corba`
`sudo apt install -qqy robotpkg-py27-qt4-hpp-gepetto-viewer`
`sudo apt install -qqy robotpkg-pyXX-qt4-hpp-gepetto-viewer`
2. Install the pinocchio bindings
`sudo apt install -qqy robotpkg-py27-pinocchio`
`sudo apt install -qqy robotpkg-pyXX-pinocchio`
3. Install the dae extension for osg
......@@ -59,21 +62,47 @@ If you are planning to use the visualization tools used by the Gepetto team, alo
To see the planner in action, one example from our IJRR submission with HyQ is available. Examples with HRP-2 are also provided, though they can only be executed if you have access to HRP-2 model.
- If you installed the planner form binaries, you need to download the scripts as explained here. Otherwise you can find them directly in script/scenarios/demos folder. For the binary proceudre, create a folder and cd in to it, then type
`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`
- The planning is decomposed in two phases / scripts. First, a root path is computed (\*_path.py files). Then, the contacts are generated along the computed path (\*_interp.py files). The scripts are located in the folder /scripts/scenarios/demos.
- To see the different steps of the process run
``$ ./run.sh darpa_hyq.py`
The script include comments explaining the different calls to the library. You can call the different methods a() ... d() to see the different steps of the planning.
- You can find the scripts in your install directory, in `lib/pythonXX/dist-packages/hpp/corbaserver/rbprm/scenarios/demos` folder.
- The planning is decomposed in two phases / scripts. First, a root path is computed (`\*_path.py files`). Then, the contacts are generated along the computed path.
- In order to start a scenario, run:
`python -im hpp.corbaserver.rbprm demos.hyq_darpa`
Replace demos.hyq_darpa with the name of any file in the demos or memmo folder to try different scenarios.
- Once the script have been executed, you can display the results in the viewer (if you installed it):
- If it was a `\*_path.py` script, you can run:
- `planner.play_path()` to display the computed guide path
- `planner.v(planner.q_init)` or `planner.v(planner.q_goal)` to put the robot at the initial / goal position of the planning
- If it was a a contact generation script, you can run:
- `cg.display_sequence()` to display the sequence of configurations in contact computed
- `cg.display_init_config()` or `cg.display_end_config()` to put the robot at the initial / final whole body configuration
- `cg.v(cg.configs[i])` to display the i-th wholebody configuration of the sequence
- `cg.play_guide_path()` to display the guide path
## Creating a new scenario script
Start from one of the scripts in the scenarios/demos folder, eg `talos_flatGround.py`.
* All the `\*_path.py` scripts must define a class called `PathPlanner` that inherit from one of the `{robot}_path_planner` classes defined in the scenarios folder.
* In the run() method, define the environment used and the initial/goal position
* If further customization is needed, override the required methods from the parent class.
* All the contact generation scripts must define a class called `ContactGenerator` that inherit from one of the `{robot}_contact_generator` classes defined in the scenarios folder.
* In the constructor of this class, the parent constructor must be called with an instance of the desired `PathPlanner` class, defining the environment and the guide trajectory.
* This class may override any method from the parent class in order to change the default settings/choices regarding the contact generation
cmake @ ee7a773c
Compare 61e5574a...ee7a773c
Subproject commit 61e5574a0615706aab06986f6aecf665ddc31141
Subproject commit ee7a773c5c23f83dd21eb0ccfa96277e068b0456
idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
View file @ 3a39a6b3
......@@ -32,21 +32,14 @@ module hpp
/// This function can be called several times to include several ROMs (one for each limb)
/// The device automatically has an anchor joint called "base_joint" as
/// root joint.
/// \param romRobotName the name of the robot range of motion.
/// Load robot model
/// \param robotName the name of the robot range of motion,
/// \param rootJointType type of root joint among "anchor", "freeflyer",
/// "planar",
/// \param packageName Name of the ROS package containing the model,
/// \param modelName Name of the package containing the model
/// \param urdfSuffix suffix for urdf file,
/// \param urdfName name of the urdf file. It may contain
/// "file://", or "package://" prefixes.
///
/// The ros url are built as follows:
/// "package://${packageName}/urdf/${modelName}${urdfSuffix}.urdf"
/// "package://${packageName}/srdf/${modelName}${srdfSuffix}.srdf"
///
void loadRobotRomModel (in string romRobotName, in string rootJointType,
in string packageName, in string modelName,
in string urdfSuffix, in string srdfSuffix)
void loadRobotRomModel (in string robotName, in string rootJointType,
in string urdfName)
raises (Error);
......@@ -54,39 +47,34 @@ module hpp
/// The device automatically has an anchor joint called "base_joint" as
/// root joint.
/// \param trunkRobotName the name of the robot trunk used for collision.
/// \param robotName the name of the robot trunk used for collision,
/// \param rootJointType type of root joint among "anchor", "freeflyer",
/// "planar",
/// \param packageName Name of the ROS package containing the model,
/// \param modelName Name of the package containing the model
/// \param urdfSuffix suffix for urdf file,
///
/// The ros url are built as follows:
/// "package://${packageName}/urdf/${modelName}${urdfSuffix}.urdf"
/// "package://${packageName}/srdf/${modelName}${srdfSuffix}.srdf"
/// \param urdfName name of the urdf file. It may contain
/// "file://", or "package://" prefixes.
/// \param srdfName name of the srdf file. It may contain
/// "file://", or "package://" prefixes.
///
void loadRobotCompleteModel (in string trunkRobotName, in string rootJointType,
in string packageName, in string modelName,
in string urdfSuffix, in string srdfSuffix)
void loadRobotCompleteModel (in string robotName, in string rootJointType,
in string urdfName, in string srdfName)
raises (Error);
/// Load fullbody robot model
///
/// Create a RbprmFullBody object
/// The device automatically has an anchor joint called "base_joint" as
/// root joint.
/// \param trunkRobotName the name of the robot trunk used for collision.
///
/// \param robotName Name of the robot that is constructed,
/// \param rootJointType type of root joint among "anchor", "freeflyer",
/// "planar",
/// \param packageName Name of the ROS package containing the model,
/// \param modelName Name of the package containing the model
/// \param urdfSuffix suffix for urdf file,
///
/// The ros url are built as follows:
/// "package://${packageName}/urdf/${modelName}${urdfSuffix}.urdf"
/// "package://${packageName}/srdf/${modelName}${srdfSuffix}.srdf"
/// \param urdfName name of the urdf file. It may contain
/// "file://", or "package://" prefixes.
/// \param srdfName name of the srdf file. It may contain
/// "file://", or "package://" prefixes.
///
void loadFullBodyRobot (in string trunkRobotName, in string rootJointType,
in string packageName, in string modelName,
in string urdfSuffix, in string srdfSuffix, in string selectedProblem)
void loadFullBodyRobot (in string robotName, in string rootJointType,
in string urdfName, in string srdfName, in string selectedProblem)
raises (Error);
/// Create a RbprmFullBody object
......@@ -110,6 +98,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 +136,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);
/// 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);
/// 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 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 +250,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 +366,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 +377,8 @@ 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);
/// \param erasePreviousStates : if True, the method override the current state list in fullBody, otherwise it append the results
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 +666,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 +743,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 +765,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 +807,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);
......
include/hpp/corbaserver/rbprm/server.hh
View file @ 3a39a6b3
......@@ -18,36 +18,37 @@
// <http://www.gnu.org/licenses/>.
#ifndef HPP_RBPRM_CORBA_SERVER_HH
# define HPP_RBPRM_CORBA_SERVER_HH
#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/corba/template/server.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
{
public:
Server (int argc, const char *argv[], bool multiThread = false,
const std::string& poaName = "child");
~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);
public:
corba::Server <impl::RbprmBuilder>* rbprmBuilder_;
}; // class Server
} // namespace rbprm
} // namespace hpp
#endif // HPP_RBPRM_CORBA_SERVER_HH
namespace rbprm {
namespace impl {
class RbprmBuilder;
}
class HPP_RBPRM_CORBA_DLLAPI Server : public corbaServer::ServerPlugin {
public:
Server(corbaServer::Server* parent);
~Server();
/// Start corba server
/// Call hpp::corba::Server <impl::Problem>::startCorbaServer
void startCorbaServer(const std::string& contextId, const std::string& contextKind);
std::string name() const;
::CORBA::Object_ptr servant (const std::string& name) const;
public:
corba::Server<impl::RbprmBuilder>* rbprmBuilder_;
}; // class Server
} // namespace rbprm
} // namespace hpp
#endif // HPP_RBPRM_CORBA_SERVER_HH
matlab/effectorRomToObj.m 0 → 100755
View file @ 3a39a6b3
% Open it on blender with default axis setting and export it with x forward.
% Use decimate to reduce the number of faces, don't forget to triangulate faces before exporting
function [] = effectorRomToObj(filename, outname)
delimiterIn = ',';
headerlinesIn = 0;
points = importdata(filename,delimiterIn,headerlinesIn);
k = convhull(points);
vertface2obj(points,k,outname)
matlab/vertface2obj.m 0 → 100755
View file @ 3a39a6b3
function vertface2obj(v,f,name)
% VERTFACE2OBJ Save a set of vertice coordinates and faces as a Wavefront/Alias Obj file
% VERTFACE2OBJ(v,f,fname)
% v is a Nx3 matrix of vertex coordinates.
% f is a Mx3 matrix of vertex indices.
% fname is the filename to save the obj file.
fid = fopen(name,'w');
for i=1:size(v,1)
fprintf(fid,'v %f %f %f\n',v(i,1),v(i,2),v(i,3));
end
fprintf(fid,'g foo\n');
for i=1:size(f,1);
fprintf(fid,'f %d %d %d\n',f(i,1),f(i,2),f(i,3));
end
fprintf(fid,'g\n');
fclose(fid);
package.xml 0 → 100644
View file @ 3a39a6b3
<?xml version='1.0'?>
<package format='2'>
<name>hpp-rbprm-corba</name>
<version>4.12.0</version>
<description>RB-PRM planner for HPP</description>
<maintainer email='guilhem.saurel@laas.fr'>Guilhem Saurel</maintainer>
<license>BSD</license>
<author>Pierre Fernbach</author>
<author>Steve Tonneau</author>
<buildtool_depend>catkin</buildtool_depend>
</package>
script/ANYmal/anymal_flatGround.py 0 → 100644
View file @ 3a39a6b3
from hpp.corbaserver.rbprm.anymal import Robot
from hpp.gepetto import Viewer
from hpp.corbaserver.rbprm.tools.display_tools import *
import time
print("Plan guide trajectory ...")
import anymal_flatGround_path as tp
print("Done.")
import time
pId = tp.ps.numberPaths() -1
fullBody = Robot ()
# Set the bounds for the root, take slightly larger bounding box than during root planning
root_bounds = tp.root_bounds[::]
root_bounds[0] -= 0.5
root_bounds[1] += 0.5
root_bounds[2] -= 0.5
root_bounds[3] += 0.5
root_bounds[-1] = 0.6
root_bounds[-2] = 0.3
fullBody.setJointBounds ("root_joint", root_bounds)
fullBody.setVeryConstrainedJointsBounds()
# 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 = fullBody.referenceConfig_asymetric[::]+[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()
dict_heuristic = {fullBody.rLegId:"fixedStep04", fullBody.lLegId:"fixedStep04", fullBody.rArmId:"static", fullBody.lArmId:"static"}
#fullBody.loadAllLimbs("static","ReferenceConfiguration",nbSamples=50000,disableEffectorCollision=False)
fullBody.loadAllLimbs(dict_heuristic,"ReferenceConfiguration",nbSamples=100000,disableEffectorCollision=False)
tGenerate = time.time() - tStart
print("Done.")
print("Databases generated in : "+str(tGenerate)+" s")
fullBody.setReferenceConfig(q_ref)
#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_ref[::]; q_goal[0:7] = tp.ps.configAtParam(pId,tp.ps.pathLength(pId))[0:7]
dir_init = [0,0,0]
acc_init = tp.ps.configAtParam(pId,0.)[tp.indexECS+3:tp.indexECS+6]
dir_goal = tp.ps.configAtParam(pId,tp.ps.pathLength(pId))[tp.indexECS:tp.indexECS+3]
acc_goal = [0,0,0]
robTreshold = 5
# 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] = fullBody.referenceConfig_asymetric[2]
q_init[2] = q_ref[2]
q_goal[2] = q_ref[2]
normals = [[0.,0.,1.] for _ in range(4)]
fullBody.setStaticStability(True)
fullBody.setCurrentConfig (q_init)
v(q_init)
fullBody.setStartState(q_init,Robot.limbs_names,normals)
fullBody.setEndState(q_goal,Robot.limbs_names,normals)
fullBody.setCurrentConfig (q_goal)
v(q_goal)
v.addLandmark('anymal/base',0.3)
print("Generate contact plan ...")
tStart = time.time()
configs = fullBody.interpolate(0.001,pathId=pId,robustnessTreshold = robTreshold, filterStates = True,testReachability=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()
script/scenarios/demos/talos_navBauzil_path.py → script/ANYmal/anymal_flatGround_path.py
View file @ 3a39a6b3
from hpp.corbaserver.rbprm.talos_abstract import Robot
from hpp.corbaserver.rbprm.anymal_abstract import Robot
from hpp.gepetto import Viewer
from hpp.corbaserver import Client
from hpp.corbaserver import ProblemSolver
......@@ -7,23 +7,21 @@ import time
vMax = 0.3# linear velocity bound for the root
aMax = 0.1 # linear acceleration bound for the root
aMax = 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
root_bounds = [-1.5,3,0.,3.3, 0.98, 0.98]
root_bounds = [-3,3,-3,3, 0.4, 0.5]
rbprmBuilder.setJointBounds ("root_joint", root_bounds)
# As this scenario only consider walking, we fix the DOF of the torso :
rbprmBuilder.setJointBounds ('torso_1_joint', [0,0])
rbprmBuilder.setJointBounds ('torso_2_joint', [0.006761,0.006761])
# 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(['talos_lleg_rom','talos_rleg_rom'])
rbprmBuilder.setAffordanceFilter('talos_lleg_rom', ['Support',])
rbprmBuilder.setAffordanceFilter('talos_rleg_rom', ['Support'])
rbprmBuilder.setFilter(rbprmBuilder.urdfNameRom)
for rom in rbprmBuilder.urdfNameRom :
rbprmBuilder.setAffordanceFilter(rom, ['Support'])
# We also bound the rotations of the torso. (z, y, x)
rbprmBuilder.boundSO3([-4.,4.,-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
......@@ -38,9 +36,9 @@ ps = ProblemSolver( rbprmBuilder )
ps.setParameter("Kinodynamic/velocityBound",vMax)
ps.setParameter("Kinodynamic/accelerationBound",aMax)
# force the orientation of the trunk to match the direction of the motion
ps.setParameter("Kinodynamic/forceOrientation",True)
ps.setParameter("DynamicPlanner/sizeFootX",0.2)
ps.setParameter("DynamicPlanner/sizeFootY",0.12)
ps.setParameter("Kinodynamic/forceAllOrientation",True)
ps.setParameter("DynamicPlanner/sizeFootX",0.01)
ps.setParameter("DynamicPlanner/sizeFootY",0.01)
ps.setParameter("DynamicPlanner/friction",mu)
# sample only configuration with null velocity and acceleration :
ps.setParameter("ConfigurationShooter/sampleExtraDOF",False)
......@@ -54,22 +52,22 @@ vf = ViewerFactory (ps)
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool ()
afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
afftool.loadObstacleModel ("hpp_environments", "multicontact/floor_bauzil", "planning", vf)
afftool.loadObstacleModel ("hpp_environments", "multicontact/ground", "planning", vf)
v = vf.createViewer(displayArrows = True)
#afftool.visualiseAffordances('Support', v, v.color.lightBrown)
v.addLandmark(v.sceneName,1)
#v.addLandmark(v.sceneName,1)
# Setting initial configuration
q_init = rbprmBuilder.getCurrentConfig ();
q_init [0:3] = [-0.9,1.5,0.98]
q_init[-6:-3] = [0.07,0,0]
q_init [0:3] = [0,0,0.465]
q_init[-6:-3] = [0,0,0]
v (q_init)
ps.setInitialConfig (q_init)
# set goal config
rbprmBuilder.setCurrentConfig (q_init)
q_goal = q_init [::]
q_goal[0:3] = [2,2.6,0.98]
q_goal[-6:-3] = [0.1,0,0]
q_goal[0:3] = [1.,0,0.465]
q_goal[-6:-3] = [0,0,0]
v(q_goal)
......@@ -86,17 +84,17 @@ ps.selectPathPlanner("DynamicPlanner")
# Solve the planning problem :
t = ps.solve ()
print "Guide planning time : ",t
print("Guide planning time : ",t)
# display solution :
from hpp.gepetto import PathPlayer
pp = PathPlayer (v)
pp.dt=0.1
pp.displayVelocityPath(1)
v.client.gui.setVisibility("path_1_root","ALWAYS_ON_TOP")
#pp.displayVelocityPath(1)
#v.client.gui.setVisibility("path_1_root","ALWAYS_ON_TOP")
pp.dt = 0.01
pp(1)
#pp(1)
# move the robot out of the view before computing the contacts
q_far = q_init[::]
......
script/ANYmal/anymal_modular_palet_flat.py 0 → 100644
View file @ 3a39a6b3
from hpp.corbaserver.rbprm.anymal import Robot
from hpp.gepetto import Viewer
from tools.display_tools import *
import time
print("Plan guide trajectory ...")
import scenarios.sandbox.ANYmal.anymal_modular_palet_flat_path as tp
#Robot.urdfSuffix += "_safeFeet"
statusFilename = tp.statusFilename
pId = tp.pid
f = open(statusFilename,"a")
if tp.ps.numberPaths() > 0 :
print("Path planning OK.")
f.write("Planning_success: True"+"\n")
f.close()
else :
print("Error during path planning")
f.write("Planning_success: False"+"\n")
f.close()
import sys
sys.exit(1)
fullBody = Robot ()
# Set the bounds for the root
rootBounds = tp.rootBounds[::]
rootBounds[0] -= 0.2
rootBounds[1] += 0.2
rootBounds[2] -= 0.5
rootBounds[3] += 0.5
rootBounds[4] -= 0.2
rootBounds[5] += 0.2
fullBody.setJointBounds ("root_joint", rootBounds)
fullBody.setConstrainedJointsBounds()
# 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,-tp.vMax,tp.vMax,-tp.aMax,tp.aMax,-tp.aMax,tp.aMax,-tp.aMaxZ,tp.aMaxZ])
ps = tp.ProblemSolver( fullBody )
ps.setParameter("Kinodynamic/velocityBound",tp.vMax)
ps.setParameter("Kinodynamic/accelerationBound",tp.aMax)
ps.setParameter("FullBody/frictionCoefficient",tp.mu)
#load the viewer
try :
v = tp.Viewer (ps,viewerClient=tp.v.client, displayCoM = True)
except Exception:
print("No viewer started !")
class FakeViewer():
def __init__(self):
return
def __call__(self,q):
return
def addLandmark(self,a,b):
return
v = FakeViewer()
# 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.usePosturalTaskContactCreation(True)
fullBody.setCurrentConfig (q_init)
print("Generate limb DB ...")
tStart = time.time()
# generate databases :
fullBody.loadAllLimbs("static","ReferenceConfiguration",nbSamples=100000,disableEffectorCollision=False)
tGenerate = time.time() - tStart
print("Done.")
print("Databases generated in : "+str(tGenerate)+" s")
v.addLandmark('anymal/base_0',0.2)
#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]
vel_init = [0,0,0]
robTreshold =0.
# 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.13
q_goal[2] = q_ref[2] + 0.15
normals = [[0.,0.,1.] for _ in range(4)]
fullBody.setStaticStability(True)
fullBody.setCurrentConfig (q_init)
v(q_init)
fullBody.setStartState(q_init,Robot.limbs_names,normals)
fullBody.setEndState(q_goal,Robot.limbs_names,normals)
fullBody.setCurrentConfig (q_goal)
v(q_goal)
print("Generate contact plan ...")
tStart = time.time()
configs = fullBody.interpolate(0.001,pathId=pId,robustnessTreshold = robTreshold, filterStates = True,testReachability=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)
if len(configs) < 2 :
cg_success = False
print("Error during contact generation.")
else:
cg_success = True
print("Contact generation Done.")
if abs(configs[-1][0] - tp.q_goal[0]) < 0.01 and abs(configs[-1][1]- tp.q_goal[1]) < 0.01 and (len(fullBody.getContactsVariations(len(configs)-2,len(configs)-1))==1):
print("Contact generation successful.")
cg_reach_goal = True
else:
print("Contact generation failed to reach the goal.")
cg_reach_goal = False
f = open(statusFilename,"a")
f.write("cg_success: "+str(cg_success)+"\n")
f.write("cg_reach_goal: "+str(cg_reach_goal)+"\n")
f.close()
if (not cg_success):
import sys
sys.exit(1)
#beginId = 2
# put back original bounds for wholebody methods
fullBody.resetJointsBounds()
#displayContactSequence(v,configs)
script/ANYmal/anymal_modular_palet_flat_path.py 0 → 100644
View file @ 3a39a6b3
from hpp.corbaserver.rbprm.anymal_abstract import Robot
Robot.urdfName += "_large"
from hpp.gepetto import Viewer
from hpp.corbaserver import ProblemSolver
from pinocchio import Quaternion
import numpy as np
import time
import math
statusFilename = "infos.log"
REF_Z_VALUE = 0.47
Z_VALUE_LOGS = REF_Z_VALUE + 0.13
Z_VALUE_PALET = REF_Z_VALUE + 0.165
Z_VALUE_WOOD = REF_Z_VALUE + 0.15
Y_VALUE = -0.13
vInit = 0.01
vMax = 0.5# linear velocity bound for the root
aMax = 0.5# linear acceleration bound for the root
aMaxZ = 5.
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 ()
# Define bounds for the root : bounding box of the scenario
rootBounds = [-1.201,1.11, Y_VALUE-0.001, Y_VALUE+0.001, Z_VALUE_LOGS - 0.001, Z_VALUE_PALET + 0.06]
rbprmBuilder.setJointBounds ("root_joint", rootBounds)
# 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.urdfNameRom)
for rom in rbprmBuilder.urdfNameRom :
rbprmBuilder.setAffordanceFilter(rom, ['Support'])
# We also bound the rotations of the torso. (z, y, x)
rbprmBuilder.boundSO3([-0.01,0.01,-0.01,0.01,-0.01,0.01])
# 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,-2.,2.,-aMax,aMax,-aMax,aMax,-aMaxZ,aMaxZ])
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("Kinodynamic/verticalAccelerationBound",aMaxZ)
#ps.setParameter("Kinodynamic/synchronizeVerticalAxis",False)
#ps.setParameter("Kinodynamic/forceYawOrientation",True)
ps.setParameter("DynamicPlanner/sizeFootX",0.01)
ps.setParameter("DynamicPlanner/sizeFootY",0.01)
ps.setParameter("DynamicPlanner/friction",mu)
# sample only configuration with null velocity and acceleration :
ps.setParameter("ConfigurationShooter/sampleExtraDOF",False)
ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops",100)
# 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.setMinimumArea('Support',0.03)
afftool.loadObstacleModel ("hpp_environments", "ori/modular_palet_flat", "planning", vf,reduceSizes=[0.11,0,0])
#afftool.loadObstacleModel ("hpp_environments", "ori/modular_palet_low", "planning", vf)
try :
v = vf.createViewer(displayArrows = True)
except Exception:
print("No viewer started !")
class FakeViewer():
def __init__(self):
return
def __call__(self,q):
return
v = FakeViewer()
#afftool.visualiseAffordances('Support', v, v.color.lightBrown)
v.addLandmark(v.sceneName,1)
# Choosing RBPRM shooter and path validation methods.
ps.selectConfigurationShooter("RbprmShooter")
ps.selectPathValidation("RbprmPathValidation",0.05)
# Choosing kinodynamic methods :
ps.selectSteeringMethod("RBPRMKinodynamic")
ps.selectDistance("Kinodynamic")
ps.selectPathPlanner("DynamicPlanner")
ps.addPathOptimizer ("RandomShortcutDynamic")
#ps.addPathOptimizer ("RandomShortcut")
# init to beginning of palet
q_init = rbprmBuilder.getCurrentConfig ();
q_init[2] = Z_VALUE_LOGS
q_init[0:2] = [-1.2,Y_VALUE]
q_init[-6] = vInit
v(q_init)
q_goal = q_init[::]
q_goal[0] = -0.55
q_goal[2] = Z_VALUE_PALET + 0.03
q_goal[3:7] = [ 0, -0.0499792, 0, 0.9987503 ]
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
# Solve the planning problem :
t = ps.solve()
print("Guide planning done in "+str(t))
pidBegin = ps.numberPaths()-1
## middle part on palet
q_init = q_goal[::]
q_goal[3:7] = [ 0, 0.0499792, 0, 0.9987503 ]
q_goal[0] = 0.55
ps.resetGoalConfigs()
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
# Solve the planning problem :
t = ps.solve()
print("Guide planning done in "+str(t))
pidMiddle = ps.numberPaths()-1
## last part on wood floor
q_init = q_goal[::]
q_goal[3:7] = [ 0, 0, 0, 1. ]
q_goal[0] = 1.1
q_goal[2] = Z_VALUE_WOOD
ps.resetGoalConfigs()
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
# Solve the planning problem :
t = ps.solve()
print("Guide planning done in "+str(t))
pidLast = ps.numberPaths()-1
# concatenate paths :
pid = pidBegin
ps.concatenatePath(pid,pidMiddle)
ps.concatenatePath(pid,pidLast)
"""
for i in range(5):
print "optimize pass : ",i
ps.optimizePath(pid)
pid = ps.numberPaths()-1
"""
try :
# display solution :
from hpp.gepetto import PathPlayer
pp = PathPlayer (v)
pp.dt=0.1
pp.displayPath(pid)#pp.displayVelocityPath(0) #
v.client.gui.setVisibility("path_"+str(pid)+"_root","ALWAYS_ON_TOP")
pp.dt=0.01
pp(pid)
except Exception:
pass
# move the robot out of the view before computing the contacts
q_far = q_init[::]
q_far[2] = -2
v(q_far)
script/ANYmal/anymal_modular_palet_low.py 0 → 100644
View file @ 3a39a6b3
from hpp.corbaserver.rbprm.anymal import Robot
from hpp.gepetto import Viewer
from hpp.corbaserver.rbprm.tools.display_tools import *
import time
print("Plan guide trajectory ...")
import scenarios.sandbox.ANYmal.anymal_modular_palet_low_obstacles_path as tp
#Robot.urdfSuffix += "_safeFeet"
statusFilename = tp.statusFilename
pId = tp.pid
f = open(statusFilename,"a")
if tp.ps.numberPaths() > 0 :
print("Path planning OK.")
f.write("Planning_success: True"+"\n")
f.close()
else :
print("Error during path planning")
f.write("Planning_success: False"+"\n")
f.close()
import sys
sys.exit(1)
fullBody = Robot ()
# Set the bounds for the root
rootBounds = tp.rootBounds[::]
rootBounds[0] -= 0.2
rootBounds[1] += 0.2
rootBounds[2] -= 0.5
rootBounds[3] += 0.5
rootBounds[4] -= 0.2
rootBounds[5] += 0.2
fullBody.setJointBounds ("root_joint", rootBounds)
fullBody.setConstrainedJointsBounds()
# 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,-tp.vMax,tp.vMax,-tp.aMax,tp.aMax,-tp.aMax,tp.aMax,-tp.aMaxZ,tp.aMaxZ])
ps = tp.ProblemSolver( fullBody )
ps.setParameter("Kinodynamic/velocityBound",tp.vMax)
ps.setParameter("Kinodynamic/accelerationBound",tp.aMax)
ps.setParameter("FullBody/frictionCoefficient",tp.mu)
#load the viewer
try :
v = tp.Viewer (ps,viewerClient=tp.v.client, displayCoM = True)
except Exception:
print("No viewer started !")
class FakeViewer():
def __init__(self):
return
def __call__(self,q):
return
def addLandmark(self,a,b):
return
v = FakeViewer()
# 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.usePosturalTaskContactCreation(True)
fullBody.setCurrentConfig (q_init)
print("Generate limb DB ...")
tStart = time.time()
# generate databases :
fullBody.loadAllLimbs("static","ReferenceConfiguration",nbSamples=100000,disableEffectorCollision=False)
tGenerate = time.time() - tStart
print("Done.")
print("Databases generated in : "+str(tGenerate)+" s")
v.addLandmark('anymal/base_0',0.2)
#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]
vel_init = [0,0,0]
robTreshold = 3
# 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.13
q_goal[2] = q_ref[2] + 0.15
normals = [[0.,0.,1.] for _ in range(4)]
fullBody.setStaticStability(True)
fullBody.setCurrentConfig (q_init)
v(q_init)
fullBody.setStartState(q_init,Robot.limbs_names,normals)
fullBody.setEndState(q_goal,Robot.limbs_names,normals)
fullBody.setCurrentConfig (q_goal)
v(q_goal)
print("Generate contact plan ...")
tStart = time.time()
configs = fullBody.interpolate(0.001,pathId=pId,robustnessTreshold = robTreshold, filterStates = True,testReachability=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)
if len(configs) < 2 :
cg_success = False
print("Error during contact generation.")
else:
cg_success = True
print("Contact generation Done.")
if abs(configs[-1][0] - tp.q_goal[0]) < 0.01 and abs(configs[-1][1]- tp.q_goal[1]) < 0.01 and (len(fullBody.getContactsVariations(len(configs)-2,len(configs)-1))==1):
print("Contact generation successful.")
cg_reach_goal = True
else:
print("Contact generation failed to reach the goal.")
cg_reach_goal = False
f = open(statusFilename,"a")
f.write("cg_success: "+str(cg_success)+"\n")
f.write("cg_reach_goal: "+str(cg_reach_goal)+"\n")
f.close()
if (not cg_success):
import sys
sys.exit(1)
#beginId = 2
# put back original bounds for wholebody methods
fullBody.resetJointsBounds()
#displayContactSequence(v,configs)
script/ANYmal/anymal_modular_palet_low_obstacles_path.py 0 → 100644
View file @ 3a39a6b3
from hpp.corbaserver.rbprm.anymal_abstract import Robot
Robot.urdfName += "_large"
from hpp.gepetto import Viewer
from hpp.corbaserver import ProblemSolver
from pinocchio import Quaternion
import numpy as np
import time
import math
statusFilename = "infos.log"
REF_Z_VALUE = 0.47
Z_VALUE_LOGS = REF_Z_VALUE + 0.13
Z_VALUE_PALET = REF_Z_VALUE + 0.165
Z_VALUE_WOOD = REF_Z_VALUE + 0.15
Y_VALUE = -0.13
vInit = 0.2
vMax = 0.5# linear velocity bound for the root
aMax = 0.8# linear acceleration bound for the root
aMaxZ = 5.
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 ()
# Define bounds for the root : bounding box of the scenario
rootBounds = [-1.201,1.11, Y_VALUE-0.3, Y_VALUE+0.3, Z_VALUE_LOGS - 0.001, Z_VALUE_PALET + 0.06]
rbprmBuilder.setJointBounds ("root_joint", rootBounds)
# 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.urdfNameRom)
for rom in rbprmBuilder.urdfNameRom :
rbprmBuilder.setAffordanceFilter(rom, ['Support'])
# We also bound the rotations of the torso. (z, y, x)
rbprmBuilder.boundSO3([-0.3,0.3,-0.01,0.01,-0.01,0.01])
# 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,-2.,2.,-aMax,aMax,-aMax,aMax,-aMaxZ,aMaxZ])
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("Kinodynamic/verticalAccelerationBound",aMaxZ)
#ps.setParameter("Kinodynamic/synchronizeVerticalAxis",False)
ps.setParameter("Kinodynamic/forceYawOrientation",True)
ps.setParameter("DynamicPlanner/sizeFootX",0.01)
ps.setParameter("DynamicPlanner/sizeFootY",0.01)
ps.setParameter("DynamicPlanner/friction",mu)
# sample only configuration with null velocity and acceleration :
ps.setParameter("ConfigurationShooter/sampleExtraDOF",False)
ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops",500)
# 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.setMinimumArea('Support',0.03)
afftool.loadObstacleModel ("hpp_environments", "ori/modular_palet_low_obstacles", "planning", vf,reduceSizes=[0.1,0,0])
#afftool.loadObstacleModel ("hpp_environments", "ori/modular_palet_low", "planning", vf)
try :
v = vf.createViewer(displayArrows = True)
except Exception:
print("No viewer started !")
class FakeViewer():
def __init__(self):
return
def __call__(self,q):
return
v = FakeViewer()
#afftool.visualiseAffordances('Support', v, v.color.lightBrown)
v.addLandmark(v.sceneName,1)
# Choosing RBPRM shooter and path validation methods.
ps.selectConfigurationShooter("RbprmShooter")
ps.selectPathValidation("RbprmPathValidation",0.05)
# Choosing kinodynamic methods :
ps.selectSteeringMethod("RBPRMKinodynamic")
ps.selectDistance("Kinodynamic")
ps.selectPathPlanner("DynamicPlanner")
ps.addPathOptimizer ("RandomShortcutDynamic")
#ps.addPathOptimizer ("RandomShortcut")
# init to beginning of palet
q_init = rbprmBuilder.getCurrentConfig ();
q_init[2] = Z_VALUE_LOGS
q_init[0:2] = [-1.2,Y_VALUE]
q_init[-6] = vInit
v(q_init)
q_goal = q_init[::]
q_goal[0] = -0.55
q_goal[2] = Z_VALUE_PALET + 0.03
q_goal[3:7] = [ 0, -0.0499792, 0, 0.9987503 ]
q_goal[-6] = vInit
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
# Solve the planning problem :
t = ps.solve()
print("Guide planning done in "+str(t))
pidBegin = ps.numberPaths()-1
## middle part on palet
q_init = q_goal[::]
q_goal[3:7] = [ 0, 0.0499792, 0, 0.9987503 ]
q_goal[0] = 0.55
q_goal[1] = -Y_VALUE
q_goal[-6] = vInit
ps.resetGoalConfigs()
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
rbprmBuilder.setJointBounds ("root_joint", [q_init[0],q_goal[0],-0.2,0.2,q_init[2],q_init[2]])
# Solve the planning problem :
t = ps.solve()
print("Guide planning done in "+str(t))
#v.solveAndDisplay('rm',2,0.001)
pidMiddle = ps.numberPaths()-1
rbprmBuilder.setJointBounds ("root_joint", rootBounds)
## last part on wood floor
q_init = q_goal[::]
q_goal[1] = Y_VALUE
q_goal[3:7] = [ 0, 0, 0, 1. ]
q_goal[0] = 1.1
q_goal[2] = Z_VALUE_WOOD
q_goal[-6] = vInit
ps.resetGoalConfigs()
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
# Solve the planning problem :
t = ps.solve()
print("Guide planning done in "+str(t))
pidLast = ps.numberPaths()-1
# concatenate paths :
pid = pidBegin
ps.concatenatePath(pid,pidMiddle)
ps.concatenatePath(pid,pidLast)
"""
ps.optimizePath(pid)
pid = ps.numberPaths()-1
"""
try :
# display solution :
from hpp.gepetto import PathPlayer
pp = PathPlayer (v)
pp.dt=0.1
pp.displayPath(pid)#pp.displayVelocityPath(0) #
v.client.gui.setVisibility("path_"+str(pid)+"_root","ALWAYS_ON_TOP")
pp.dt=0.01
pp(pid)
except Exception:
pass
# move the robot out of the view before computing the contacts
q_far = q_init[::]
q_far[2] = -2
v(q_far)
script/ANYmal/anymal_modular_palet_low_path.py 0 → 100644
View file @ 3a39a6b3
from hpp.corbaserver.rbprm.anymal_abstract import Robot
Robot.urdfName += "_large"
from hpp.gepetto import Viewer
from hpp.corbaserver import ProblemSolver
from pinocchio import Quaternion
import numpy as np
import time
import math
statusFilename = "infos.log"
REF_Z_VALUE = 0.47
Z_VALUE_LOGS = REF_Z_VALUE + 0.13
Z_VALUE_PALET = REF_Z_VALUE + 0.165
Z_VALUE_WOOD = REF_Z_VALUE + 0.15
Y_VALUE = -0.13
vInit = 0.01
vMax = 0.5# linear velocity bound for the root
aMax = 0.5# linear acceleration bound for the root
aMaxZ = 5.
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 ()
# Define bounds for the root : bounding box of the scenario
rootBounds = [-1.201,1.11, Y_VALUE-0.001, Y_VALUE+0.001, Z_VALUE_LOGS - 0.001, Z_VALUE_PALET + 0.06]
rbprmBuilder.setJointBounds ("root_joint", rootBounds)
# 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.urdfNameRom)
for rom in rbprmBuilder.urdfNameRom :
rbprmBuilder.setAffordanceFilter(rom, ['Support'])
# We also bound the rotations of the torso. (z, y, x)
rbprmBuilder.boundSO3([-0.01,0.01,-0.01,0.01,-0.01,0.01])
# 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,-2.,2.,-aMax,aMax,-aMax,aMax,-aMaxZ,aMaxZ])
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("Kinodynamic/verticalAccelerationBound",aMaxZ)
#ps.setParameter("Kinodynamic/synchronizeVerticalAxis",False)
#ps.setParameter("Kinodynamic/forceYawOrientation",True)
ps.setParameter("DynamicPlanner/sizeFootX",0.01)
ps.setParameter("DynamicPlanner/sizeFootY",0.01)
ps.setParameter("DynamicPlanner/friction",mu)
# sample only configuration with null velocity and acceleration :
ps.setParameter("ConfigurationShooter/sampleExtraDOF",False)
ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops",100)
# 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.setMinimumArea('Support',0.03)
afftool.loadObstacleModel ("hpp_environments", "ori/modular_palet_low_collision", "planning", vf,reduceSizes=[0.1,0,0])
#afftool.loadObstacleModel ("hpp_environments", "ori/modular_palet_low", "planning", vf)
try :
v = vf.createViewer(displayArrows = True)
except Exception:
print("No viewer started !")
class FakeViewer():
def __init__(self):
return
def __call__(self,q):
return
v = FakeViewer()
#afftool.visualiseAffordances('Support', v, v.color.lightBrown)
v.addLandmark(v.sceneName,1)
# Choosing RBPRM shooter and path validation methods.
ps.selectConfigurationShooter("RbprmShooter")
ps.selectPathValidation("RbprmPathValidation",0.05)
# Choosing kinodynamic methods :
#ps.selectSteeringMethod("RBPRMKinodynamic")
#ps.selectDistance("Kinodynamic")
#ps.selectPathPlanner("DynamicPlanner")
#ps.addPathOptimizer ("RandomShortcutDynamic")
ps.addPathOptimizer ("RandomShortcut")
# init to beginning of palet
q_init = rbprmBuilder.getCurrentConfig ();
q_init[2] = Z_VALUE_LOGS
q_init[0:2] = [-1.2,Y_VALUE]
q_init[-6] = vInit
v(q_init)
q_goal = q_init[::]
q_goal[0] = -0.55
q_goal[2] = Z_VALUE_PALET + 0.04
q_goal[3:7] = [ 0, -0.0499792, 0, 0.9987503 ]
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
# Solve the planning problem :
t = ps.solve()
print("Guide planning done in "+str(t))
pidBegin = ps.numberPaths()-1
## middle part on palet
q_init = q_goal[::]
q_goal[3:7] = [ 0, 0.0499792, 0, 0.9987503 ]
q_goal[0] = 0.55
ps.resetGoalConfigs()
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
# Solve the planning problem :
t = ps.solve()
print("Guide planning done in "+str(t))
pidMiddle = ps.numberPaths()-1
## last part on wood floor
q_init = q_goal[::]
q_goal[3:7] = [ 0, 0, 0, 1. ]
q_goal[0] = 1.1
q_goal[2] = Z_VALUE_WOOD
ps.resetGoalConfigs()
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
# Solve the planning problem :
t = ps.solve()
print("Guide planning done in "+str(t))
pidLast = ps.numberPaths()-1
# concatenate paths :
pid = pidBegin
ps.concatenatePath(pid,pidMiddle)
ps.concatenatePath(pid,pidLast)
"""
ps.optimizePath(pid)
pid = ps.numberPaths()-1
"""
try :
# display solution :
from hpp.gepetto import PathPlayer
pp = PathPlayer (v)
pp.dt=0.1
pp.displayPath(pid)#pp.displayVelocityPath(0) #
v.client.gui.setVisibility("path_"+str(pid)+"_root","ALWAYS_ON_TOP")
pp.dt=0.01
pp(pid)
except Exception:
pass
# move the robot out of the view before computing the contacts
q_far = q_init[::]
q_far[2] = -2
v(q_far)
script/scenarios/demos/talos_navBauzil_hard.py → script/ANYmal/anymal_plinth.py
View file @ 3a39a6b3
from hpp.corbaserver.rbprm.talos import Robot
from hpp.corbaserver.rbprm.anymal import Robot
from hpp.gepetto import Viewer
from tools.display_tools import *
from hpp.corbaserver.rbprm.tools.display_tools import *
import time
print "Plan guide trajectory ..."
import talos_navBauzil_hard_path as tp
print "Done."
print("Plan guide trajectory ...")
import anymal_plinth_path as tp
print("Done.")
import time
pId = tp.ps.numberPaths() -1
fullBody = Robot ()
# Set the bounds for the root, take slightly larger bounding box than during root planning
root_bounds = tp.root_bounds[::]
root_bounds[0] -= 0.2
root_bounds[1] += 0.2
root_bounds[2] -= 0.2
root_bounds[3] += 0.2
root_bounds[-1] = 1.2
root_bounds[-2] = 0.8
root_bounds[-1] = 0.12
root_bounds[-2] = 0.3
fullBody.setJointBounds ("root_joint", root_bounds)
fullBody.setConstrainedJointsBounds()
fullBody.setJointBounds("LF_KFE",[-1.4,0.])
fullBody.setJointBounds("RF_KFE",[-1.4,0.])
fullBody.setJointBounds("LH_KFE",[0.,1.4])
fullBody.setJointBounds("RH_KFE",[0.,1.4])
# 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])
fullBody.client.robot.setExtraConfigSpaceBounds([-tp.vMax,tp.vMax,-tp.vMax,tp.vMax,-tp.vMax,tp.vMax,-tp.aMax,tp.aMax,-tp.aMax,tp.aMax,-tp.aMaxZ,tp.aMaxZ])
ps = tp.ProblemSolver( fullBody )
ps.setParameter("Kinodynamic/velocityBound",tp.vMax)
ps.setParameter("Kinodynamic/accelerationBound",tp.aMax)
......@@ -30,42 +35,33 @@ 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,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 ..."
dict_heuristic = {fullBody.rLegId:"static", fullBody.lLegId:"static", fullBody.rArmId:"fixedStep04", fullBody.lArmId:"fixedStep04"}
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)
#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)
fullBody.runLimbSampleAnalysis(fullBody.lArmId, "ReferenceConfiguration", True)
fullBody.addNonContactingLimb(fullBody.rArmId,fullBody.rarm,fullBody.rhand,5000)
fullBody.runLimbSampleAnalysis(fullBody.rArmId, "ReferenceConfiguration", True)
fullBody.loadAllLimbs(dict_heuristic,"ReferenceConfiguration",nbSamples=50000)
tGenerate = time.time() - tStart
print "Done."
print "Databases generated in : "+str(tGenerate)+" s"
print("Done.")
print("Databases generated in : "+str(tGenerate)+" s")
fullBody.setReferenceConfig(q_ref)
#define initial and final configurations :
#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)[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]
dir_init = tp.ps.configAtParam(pId,0)[tp.indexECS:tp.indexECS+3]
acc_init = tp.ps.configAtParam(pId,0)[tp.indexECS+3:tp.indexECS+6]
dir_goal = tp.ps.configAtParam(pId,tp.ps.pathLength(pId))[tp.indexECS:tp.indexECS+3]
acc_goal = [0,0,0]
robTreshold = 3
robTreshold = 2
# copy extraconfig for start and init configurations
q_init[configSize:configSize+3] = dir_init[::]
q_init[configSize+3:configSize+6] = acc_init[::]
......@@ -82,23 +78,23 @@ v(q_init)
fullBody.setCurrentConfig (q_goal)
v(q_goal)
v.addLandmark('talos/base_link',0.3)
v.addLandmark('anymal_reachability/base',0.3)
v(q_init)
# specify the full body configurations as start and goal state of the problem
fullBody.setStartState(q_init,[fullBody.lLegId,fullBody.rLegId])
fullBody.setEndState(q_goal,[fullBody.lLegId,fullBody.rLegId])
fullBody.setStartState(q_init,fullBody.limbs_names)
fullBody.setEndState(q_goal,fullBody.limbs_names)
print "Generate contact plan ..."
print("Generate contact plan ...")
tStart = time.time()
configs = fullBody.interpolate(0.01,pathId=pId,robustnessTreshold = 2, filterStates = True)
configs = fullBody.interpolate(0.002,pathId=pId,robustnessTreshold = robTreshold, filterStates = True,testReachability=True,quasiStatic=True)
tInterpolateConfigs = time.time() - tStart
print "Done."
print "number of configs :", len(configs)
raw_input("Press Enter to display the contact sequence ...")
displayContactSequence(v,configs)
print("Done. ")
print("Contact sequence computed in "+str(tInterpolateConfigs)+" s.")
print("number of configs :", len(configs))
#raw_input("Press Enter to display the contact sequence ...")
#displayContactSequence(v,configs)
fullBody.resetJointsBounds()
script/ANYmal/anymal_plinth_path.py 0 → 100644
View file @ 3a39a6b3
from hpp.corbaserver.rbprm.anymal_abstract import Robot
from hpp.gepetto import Viewer
from hpp.corbaserver import Client
from hpp.corbaserver import ProblemSolver
import time
vMax = 0.3# linear velocity bound for the root
aMax = 1. # linear acceleration bound for the root
aMaxZ=5.
extraDof = 6
mu=0.5# coefficient of friction
# Creating an instance of the helper class, and loading the robot
Robot.urdfName += '_large'
rbprmBuilder = Robot()
# Define bounds for the root : bounding box of the scenario
root_bounds = [-2,2,-0.01,0.01, 0.4, 1.2]
rbprmBuilder.setJointBounds ("root_joint", root_bounds)
# 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(rbprmBuilder.urdfNameRom)
for rom in rbprmBuilder.urdfNameRom :
rbprmBuilder.setAffordanceFilter(rom, ['Support'])
# We also bound the rotations of the torso. (z, y, x)
rbprmBuilder.boundSO3([-0.1,0.1,-0.5,0.5,-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,-vMax,vMax,-aMax,aMax,-aMax,aMax,-aMaxZ,aMaxZ])
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("Kinodynamic/verticalAccelerationBound",aMaxZ)
# force the orientation of the trunk to match the direction of the motion
ps.setParameter("Kinodynamic/forceAllOrientation",True)
ps.setParameter("DynamicPlanner/sizeFootX",0.1)
ps.setParameter("DynamicPlanner/sizeFootY",0.1)
ps.setParameter("DynamicPlanner/friction",mu)
# sample only configuration with null velocity and acceleration :
ps.setParameter("ConfigurationShooter/sampleExtraDOF",False)
ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops",100)
# 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/ori_plinth", "planning", vf,reduceSizes=[0.05,0,0])
v = vf.createViewer(displayArrows = True)
afftool.visualiseAffordances('Support', v, v.color.lightBrown)
v.addLandmark(v.sceneName,1)
# Setting initial configuration
q_init = rbprmBuilder.getCurrentConfig ();
q_init [0:3] = [-1.7,0,0.465]
q_init[-6:-3] = [0.,0,0]
v (q_init)
ps.setInitialConfig (q_init)
# set goal config
rbprmBuilder.setCurrentConfig (q_init)
q_goal = q_init [::]
q_goal[0:3] = [1.7,0,0.465]
q_goal[-6:-3] = [0.,0,0]
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)
#v.solveAndDisplay('rm',2,radiusSphere=0.01)
# display solution :
from hpp.gepetto import PathPlayer
pp = PathPlayer (v)
pp.dt=0.1
pp.displayVelocityPath(1)
v.client.gui.setVisibility("path_1_root","ALWAYS_ON_TOP")
pp.dt = 0.01
pp(1)
# move the robot out of the view before computing the contacts
q_far = q_init[::]
q_far[2] = -2
v(q_far)