├── .gitignore ├── Makefile ├── pythonsrc └── orgpmp2 │ ├── __init__.py │ └── orgpmp2.py ├── examples ├── data │ ├── robots │ │ ├── barrett-wamhand.zae │ │ ├── pr2-beta-static.zae │ │ ├── pr2_gpmp2spheres.robot.xml │ │ ├── pr2_chompspheres.robot.xml │ │ ├── barrettwam_gpmp2spheres.robot.xml │ │ ├── barrettwam_chompspheres.robot.xml │ │ ├── wam7.kinbody.xml │ │ └── barretthand.kinbody.xml │ ├── envs │ │ ├── cup.xml │ │ ├── soda.xml │ │ ├── pringles.xml │ │ ├── table.xml │ │ ├── cabinet.xml │ │ ├── milk_bottle.xml │ │ ├── lab.env.xml │ │ ├── objects.xml │ │ ├── tunnel.env.xml │ │ ├── bookshelves.env.xml │ │ ├── industrial.env.xml │ │ ├── countertop.env.xml │ │ └── stl │ │ │ └── table.stl │ └── problemsets.py ├── gpmp2_wam.py ├── save_sdf_pr2.py ├── gpmp2_wam_withtraj.py └── gpmp2_pr2.py ├── CMakeLists.txt ├── setup.py ├── doc ├── index.md ├── Parameters.md └── Usage.md ├── src ├── utils │ ├── util_shparse.h │ ├── grid_flood.h │ ├── mat.h │ ├── grid_flood.c │ ├── util_shparse.c │ ├── grid.h │ ├── kin.h │ ├── mat.c │ ├── grid.c │ └── kin.c ├── orcwrap.h ├── orgpmp2_kdata.h ├── orcwrap.cpp ├── orgpmp2.cpp ├── orgpmp2_mod.h └── orgpmp2_kdata.cpp ├── CHANGELOG ├── catkin.cmake ├── package.xml └── README.md /.gitignore: -------------------------------------------------------------------------------- 1 | *pyc 2 | *dat 3 | *~ 4 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /pythonsrc/orgpmp2/__init__.py: -------------------------------------------------------------------------------- 1 | from orgpmp2 import * -------------------------------------------------------------------------------- /examples/data/robots/barrett-wamhand.zae: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gtrll/orgpmp2/HEAD/examples/data/robots/barrett-wamhand.zae -------------------------------------------------------------------------------- /examples/data/robots/pr2-beta-static.zae: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gtrll/orgpmp2/HEAD/examples/data/robots/pr2-beta-static.zae -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # To use this file, create CMakeLists: 2 | # - rosbuild.cmake : the CMakeLists used for ROS rosbuild builds 3 | 4 | cmake_minimum_required(VERSION 2.8.3) 5 | project(orgpmp2) 6 | include(${PROJECT_SOURCE_DIR}/catkin.cmake) -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from distutils.core import setup 3 | from catkin_pkg.python_setup import generate_distutils_setup 4 | 5 | d = generate_distutils_setup( 6 | packages=['orgpmp2'], 7 | package_dir={'': 'pythonsrc'}, 8 | ) 9 | setup(**d) -------------------------------------------------------------------------------- /examples/data/envs/cup.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | stl/cup.stl 5 | stl/cup.stl 6 | 1 0 0 7 | 8 | 9 | -------------------------------------------------------------------------------- /examples/data/envs/soda.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | stl/soda.stl 5 | stl/soda.stl 6 | 0.35 0.7 0.31 7 | 8 | 9 | -------------------------------------------------------------------------------- /examples/data/envs/pringles.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | stl/pringles.stl 5 | stl/pringles.stl 6 | 0.89 0.8 0.34 7 | 8 | 9 | -------------------------------------------------------------------------------- /examples/data/envs/table.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | stl/table.stl 5 | stl/table.stl 6 | 0.8 0.6 0.16 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /examples/data/envs/cabinet.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | stl/cabinet.stl 5 | stl/cabinet.stl 6 | 0.4 0.4 0.2 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /examples/data/envs/milk_bottle.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | stl/milk_bottle.stl 5 | stl/milk_bottle.stl 6 | 0.47 0.9 0.98 7 | 8 | 9 | -------------------------------------------------------------------------------- /examples/data/envs/lab.env.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 1.207 -0.457 -1.046 4 | 0 0 1 90 5 | 6 | 7 | 0.294 0.914 -0.233 8 | 0 0 0 0 9 | 10 | -------------------------------------------------------------------------------- /doc/index.md: -------------------------------------------------------------------------------- 1 | Documentation 2 | ============= 3 | 4 | This is documentation for orgpmp2 that is a OpenRAVE plugin for the [GPMP2](https://github.com/gtrll/gpmp2) algorithm. This plugin comes with a python wrapper and its usage is described here using examples on WAM arm and PR2 robot. For a detailed description about the algorithm see [GPMP2's documentation](https://github.com/gtrll/gpmp2/wiki). 5 | 6 | - [Usage](Usage.md) 7 | - [Parameters](Parameters.md) -------------------------------------------------------------------------------- /src/utils/util_shparse.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file util_shparse.h 3 | * \brief Interface to cd_util_shparse, a simple POSIX shell argument parser. 4 | */ 5 | 6 | /* Simple POSIX shell argument parsing; 7 | * that is, turns a string like: 8 | * grep -v -e can -e "can't" -e 'Say "Hi!"' -e "do\\don't" 9 | * into an argv array like: 10 | * grep, -v, -e, can, -e, can't, -e, Say "Hi!", -e, do\don't 11 | * Characters that have special meaning are: 12 | * space characters, newline, backslash, double quote, single quote 13 | */ 14 | 15 | int cd_util_shparse(char * in, int * argcp, char *** argvp); -------------------------------------------------------------------------------- /CHANGELOG: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for orgpmp2 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.2.0 (2017-06-16) 6 | ------------------ 7 | * Fix initializing with given trajectories 8 | * Fix bug in signed distance field (sdf) generation when using some parameters 9 | * Deprecated flood fill in sdf generation 10 | * Add functionality to save sdf to 11 | * Add more robot files and WAM dataset used in benchmarks 12 | * Contributors: Mustafa Mukadam 13 | 14 | 0.1.0 (2016-06-16) 15 | ------------------ 16 | * Initial release 17 | * Contributors: Mustafa Mukadam, Jing Dong 18 | -------------------------------------------------------------------------------- /examples/data/envs/objects.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 0.01 0.83 -0.191 4 | 0 0 0 0 5 | 6 | 7 | -0.31 0.83 -0.191 8 | 0 0 0 0 9 | 10 | 11 | -0.16 0.96 -0.191 12 | 0 0 0 0 13 | 14 | 15 | 0.14 0.96 -0.191 16 | 0 0 0 0 17 | 18 | -------------------------------------------------------------------------------- /src/utils/grid_flood.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file grid_flood.h 3 | * \brief Interface to cd_grid_flood, a flood-fill implementation. 4 | */ 5 | 6 | /* requires: 7 | * #include 8 | * #include 9 | */ 10 | 11 | /* Modify the grid by performing a flood-fill starting with the given index. 12 | * Diagonal cells are considered adjacent (e.g. 8-connected in 2D). 13 | * By default, the grid does not wrap; if wrap_dim is passed, it should 14 | * have length grid.n, with non-zero elements denoting a wrapped dimension. */ 15 | int cd_grid_flood_fill(struct cd_grid * g, size_t index_start, 16 | int * wrap_dim, 17 | int (*replace)(void *, void *), void * rptr); -------------------------------------------------------------------------------- /src/orcwrap.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file orcwrap.h 3 | * \brief Interface to orcwrap, an OpenRAVE interface command parser. 4 | */ 5 | 6 | /** 7 | * requires: 8 | * #include 9 | * #include 10 | * #include 11 | */ 12 | 13 | /* This is a tiny generic library which parses OpenRAVE interface command 14 | * strings into an argument array using shell-like quoting 15 | * (using libcd's util_shparse) */ 16 | 17 | boost::function 18 | orcwrap(boost::function fn); 19 | 20 | boost::function 21 | orcwrap(const char * cmd, boost::function fn); -------------------------------------------------------------------------------- /examples/data/envs/tunnel.env.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0.3 0.02 0.3 6 | 0.65 -0.15 0.43 7 | 1 0 0 0 8 | 9 | 10 | 11 | 12 | 13 | 14 | 0.3 0.02 0.3 15 | 0.65 -0.15 1.22 16 | 1 0 0 0 17 | 18 | 19 | 20 | 21 | 22 | 23 | 0.15 0.02 0.1 24 | 0.8 -0.15 0.83 25 | 1 0 0 0 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /catkin.cmake: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | 3 | # catkin 4 | find_package(catkin REQUIRED COMPONENTS openrave_catkin) 5 | catkin_package() 6 | catkin_python_setup() 7 | 8 | # C++11 flags for GTSAM 4.0 9 | set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") 10 | 11 | # find OpenRAVE 12 | find_package(OpenRAVE REQUIRED) 13 | include_directories(${catkin_INCLUDE_DIRS}) 14 | 15 | # find GTSAM 16 | find_package(GTSAM REQUIRED) 17 | include_directories(${GTSAM_INCLUDE_DIR}) 18 | set(GTSAM_LIBRARIES gtsam) 19 | 20 | # find GPMP2 21 | find_package(gpmp2 REQUIRED) 22 | include_directories(${gpmp2_INCLUDE_DIR}) 23 | set(gpmp2_LIBRARIES gpmp2) 24 | 25 | openrave_plugin("${PROJECT_NAME}_plugin" 26 | src/orgpmp2.cpp 27 | src/orgpmp2_mod.cpp 28 | src/orgpmp2_kdata.cpp 29 | src/orcwrap.cpp 30 | src/utils/grid.c 31 | src/utils/grid_flood.c 32 | src/utils/kin.c 33 | src/utils/mat.c 34 | src/utils/util_shparse.c 35 | ) 36 | target_link_libraries("${PROJECT_NAME}_plugin" 37 | blas 38 | lapacke 39 | lapack 40 | gsl 41 | ${GTSAM_LIBRARIES} 42 | ${gpmp2_LIBRARIES} 43 | ${catkin_LIBRARIES} 44 | ) -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | orgpmp2 4 | 0.2.0 5 | 6 | OpenRAVE plugin that implements the GPMP2 trajectory optimizer using GTSAM. 7 | 8 | https://bitbucket.org/gtrll/gp_motion_planning.git 9 | Mustafa Mukadam 10 | Jing Dong 11 | Mustafa Mukadam 12 | Jing Dong 13 | GPL 14 | catkin 15 | atlas 16 | libblas-dev 17 | liblapacke 18 | libgsl 19 | openrave_catkin 20 | libgtsam 21 | libgpmp2 22 | atlas 23 | liblapacke 24 | libgsl 25 | openrave_catkin 26 | libgtsam 27 | libgpmp2 28 | -------------------------------------------------------------------------------- /src/orgpmp2_kdata.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file orgpmp2_kdata.h 3 | * \brief Interface to orgpmp2_kdata, a parser for sphere data provided 4 | * with an OpenRAVE kinbody XML file. 5 | * requires: 6 | * - openrave/openrave.h 7 | */ 8 | 9 | namespace orgpmp2 10 | { 11 | 12 | struct sphereelem 13 | { 14 | struct sphereelem * next; 15 | struct sphere * s; 16 | }; 17 | 18 | struct sphere 19 | { 20 | /* parsed from xml */ 21 | char linkname[32]; 22 | double pos[3]; 23 | double radius; 24 | }; 25 | 26 | 27 | /* the kinbody-attached data class */ 28 | class kdata : public OpenRAVE::XMLReadable 29 | { 30 | public: 31 | struct sphereelem * sphereelems; 32 | kdata(); 33 | ~kdata(); 34 | }; 35 | 36 | 37 | /* the kdata-parser */ 38 | class kdata_parser : public OpenRAVE::BaseXMLReader 39 | { 40 | public: 41 | boost::shared_ptr d; 42 | bool inside_spheres; 43 | 44 | kdata_parser(boost::shared_ptr passed_d, const OpenRAVE::AttributesList& atts); 45 | virtual OpenRAVE::XMLReadablePtr GetReadable(); 46 | virtual ProcessElement startElement(const std::string& name, const OpenRAVE::AttributesList& atts); 47 | virtual void characters(const std::string& ch); 48 | virtual bool endElement(const std::string& name); 49 | }; 50 | 51 | } /* namespace orgpmp2 */ -------------------------------------------------------------------------------- /src/utils/mat.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file mat.h 3 | * \brief Interface to cd_mat, a collection of useful matrix routines. 4 | */ 5 | 6 | #ifndef CD_MAT_H 7 | #define CD_MAT_H 8 | 9 | #include 10 | #include 11 | 12 | int cd_mat_set_zero(double * A, int m, int n); 13 | int cd_mat_set_diag(double * A, int m, int n, double value); 14 | int cd_mat_transpose(double * A, int mn); 15 | int cd_mat_fill(double * A, int m, int n, ...); 16 | int cd_mat_memcpy(double * dst, const double * src, const int m, const int n); 17 | int cd_mat_memcpy_transpose(double * dst, const double * src, int m, int n); 18 | int cd_mat_add(double * dst, const double * src, int m, int n); 19 | int cd_mat_sub(double * dst, const double * src, int m, int n); 20 | int cd_mat_scale(double * A, int m, int n, double fac); 21 | 22 | double cd_mat_trace(double * A, int m, int n); 23 | 24 | int cd_mat_cross(const double a[3], const double b[3], double res[3]); 25 | 26 | /* THIS IS UGLY! */ 27 | char * cd_mat_vec_sprintf(char * buf, double * a, int n); 28 | 29 | int cd_mat_vec_fprintf(FILE * stream, const char * prefix, 30 | const double * a, int n, const char * start, const char * dblfmt, 31 | const char * sep, const char * end, const char * suffix); 32 | 33 | int cd_mat_vec_print(const char * prefix, const double * a, int n); 34 | 35 | int cd_mat_mat_print(const char * prefix, const double * a, int m, int n); 36 | 37 | #endif /* CD_MAT_H */ -------------------------------------------------------------------------------- /src/orcwrap.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file orcwrap.cpp 3 | * \brief Implementation of orcwrap, an OpenRAVE interface command parser. 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | extern "C" { 13 | #include "utils/util_shparse.h" 14 | } 15 | 16 | #include "orcwrap.h" 17 | 18 | static bool orcwrap_call( 19 | const char * cmd, 20 | boost::function fn, 21 | std::ostream& sout, std::istream& sinput) 22 | { 23 | char * in; 24 | int argc; 25 | char ** argv; 26 | int ret; 27 | 28 | std::ostringstream oss; 29 | oss << cmd << " " << sinput.rdbuf(); 30 | in = (char *) malloc(strlen(oss.str().c_str())+1); 31 | if (!in) return false; 32 | strcpy(in, oss.str().c_str()); 33 | 34 | cd_util_shparse(in, &argc, &argv); 35 | 36 | try 37 | { 38 | ret = fn(argc, argv, sout); 39 | } 40 | catch (...) 41 | { 42 | free(in); 43 | free(argv); 44 | throw; 45 | } 46 | 47 | free(in); 48 | free(argv); 49 | return (ret == 0) ? true : false; 50 | } 51 | 52 | boost::function 53 | orcwrap(boost::function fn) 54 | { 55 | return boost::bind(orcwrap_call,"openrave_command",fn,_1,_2); 56 | } 57 | 58 | boost::function 59 | orcwrap(const char * cmd, boost::function fn) 60 | { 61 | return boost::bind(orcwrap_call,cmd,fn,_1,_2); 62 | } -------------------------------------------------------------------------------- /examples/data/envs/bookshelves.env.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0.3 0.55 0.01 6 | 0.95 0 0.20 7 | 1 0 0 0 8 | 9 | 10 | 11 | 12 | 13 | 14 | 0.3 0.55 0.01 15 | 0.95 0 0.60 16 | 1 0 0 0 17 | 18 | 19 | 20 | 21 | 22 | 23 | 0.3 0.55 0.01 24 | 0.95 0 1.00 25 | 1 0 0 0 26 | 27 | 28 | 29 | 30 | 31 | 32 | 0.3 0.55 0.01 33 | 0.95 0 1.40 34 | 1 0 0 0 35 | 36 | 37 | 38 | 39 | 40 | 41 | 0.3 0.01 0.7 42 | 0.95 0.55 0.7 43 | 1 0 0 0 44 | 45 | 46 | 47 | 48 | 49 | 50 | 0.3 0.01 0.7 51 | 0.95 -0.55 0.7 52 | 1 0 0 0 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /src/orgpmp2.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file orgpmp2.cpp 3 | * \brief orgpmp2 entry point from OpenRAVE. 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | #include "orgpmp2_kdata.h" 15 | #include "orgpmp2_mod.h" 16 | 17 | /* globals: we maintain a single kdata xml reader instance */ 18 | namespace 19 | { 20 | 21 | static boost::shared_ptr reg_reader; 22 | 23 | static OpenRAVE::BaseXMLReaderPtr rdata_parser_maker(OpenRAVE::InterfaceBasePtr ptr, const OpenRAVE::AttributesList& atts) 24 | { 25 | return OpenRAVE::BaseXMLReaderPtr(new orgpmp2::kdata_parser(boost::shared_ptr(),atts)); 26 | } 27 | 28 | } /* anonymous namespace */ 29 | 30 | void GetPluginAttributesValidated(OpenRAVE::PLUGININFO& info) 31 | { 32 | /* create the kdata xml parser interface if it does noet yet exit */ 33 | if(!reg_reader) 34 | reg_reader = OpenRAVE::RaveRegisterXMLReader(OpenRAVE::PT_KinBody,"orgpmp2",rdata_parser_maker); 35 | 36 | info.interfacenames[OpenRAVE::PT_Module].push_back("orgpmp2"); 37 | 38 | return; 39 | } 40 | 41 | OpenRAVE::InterfaceBasePtr CreateInterfaceValidated(OpenRAVE::InterfaceType type, const std::string& interfacename, std::istream& sinput, OpenRAVE::EnvironmentBasePtr penv) 42 | { 43 | if((type == OpenRAVE::PT_Module)&&(interfacename == "orgpmp2")) 44 | return OpenRAVE::InterfaceBasePtr(new orgpmp2::mod(penv)); 45 | 46 | return OpenRAVE::InterfaceBasePtr(); 47 | } 48 | 49 | OPENRAVE_PLUGIN_API void DestroyPlugin() 50 | { 51 | RAVELOG_INFO("destroying plugin\n"); 52 | return; 53 | } -------------------------------------------------------------------------------- /src/orgpmp2_mod.h: -------------------------------------------------------------------------------- 1 | /** \file orgpmp2_mod.h 2 | * \brief Interface to the orgpmp2 module, an implementation of gpmp2 3 | * with gtsam. 4 | */ 5 | 6 | /* requires: 7 | * - openrave/openrave.h 8 | * */ 9 | 10 | #include 11 | 12 | #include "orcwrap.h" 13 | 14 | using namespace gtsam; 15 | 16 | namespace orgpmp2 17 | { 18 | 19 | struct sdf; 20 | 21 | /* the module itself */ 22 | class mod : public OpenRAVE::ModuleBase 23 | { 24 | public: 25 | OpenRAVE::EnvironmentBasePtr e; /* filled on module creation */ 26 | int n_sdfs; 27 | struct sdf * sdfs; 28 | gpmp2::SignedDistanceField gtsam_sdf; 29 | 30 | int viewspheres(int argc, char * argv[], std::ostream& sout); 31 | int computedistancefield(int argc, char * argv[], std::ostream& sout); 32 | int create(int argc, char * argv[], std::ostream& sout); 33 | int gettraj(int argc, char * argv[], std::ostream& sout); 34 | int destroy(int argc, char * argv[], std::ostream& sout); 35 | 36 | mod(OpenRAVE::EnvironmentBasePtr penv) : OpenRAVE::ModuleBase(penv) 37 | { 38 | __description = "orgpmp2: openrave wrapper for gpmp2"; 39 | RegisterCommand("viewspheres",orcwrap(boost::bind(&mod::viewspheres,this,_1,_2,_3)),"view spheres"); 40 | RegisterCommand("computedistancefield",orcwrap(boost::bind(&mod::computedistancefield,this,_1,_2,_3)),"compute distance field"); 41 | RegisterCommand("create",orcwrap(boost::bind(&mod::create,this,_1,_2,_3)),"create a gpmp2 run"); 42 | RegisterCommand("gettraj",orcwrap(boost::bind(&mod::gettraj,this,_1,_2,_3)),"get trajectory of gpmp2 run"); 43 | RegisterCommand("destroy",orcwrap(boost::bind(&mod::destroy,this,_1,_2,_3)),"destroy gpmp2 run"); 44 | 45 | this->e = penv; 46 | this->n_sdfs = 0; 47 | this->sdfs = 0; 48 | } 49 | virtual ~mod() {} 50 | void Destroy() { RAVELOG_INFO("module unloaded from environment\n"); } 51 | /* This is called on e.LoadProblem(m, 'command') */ 52 | int main(const std::string& cmd) { RAVELOG_INFO("module init cmd: %s\n", cmd.c_str()); return 0; } 53 | }; 54 | 55 | void run_destroy(struct run * r); 56 | 57 | } /* namespace orgpmp2 */ -------------------------------------------------------------------------------- /doc/Parameters.md: -------------------------------------------------------------------------------- 1 | Parameters 2 | ========================================= 3 | Here we describe parameters associated with orgpmp2 and explain how to set them. 4 | 5 | 6 | Signed Distance Field Parameters 7 | ----- 8 | - **centroid**: This is the centroid of the 3D SDF cube. Set this as the base of the robot or arm being planned for. 9 | - **extents**: This is the extent of the 3D SDF cube from the centroid in +/- x, y, and z directions. 10 | - **res**: This is the resolution parameter and defines the cell size of the SDF cube. For environments with small obstacles pick a finer resolution. 11 | 12 | Note that SDF computation takes longer with smaller cell size and larger extents. All dimensions are in meters. 13 | 14 | 15 | GPMP2 parameters 16 | ----- 17 | More information on parameters associated with the GPMP2 algorithm can be found in [GPMP2's documentation](https://github.com/gtrll/gpmp2/wiki). 18 | 19 | - **robot**: openrave robot object. 20 | - **end_conf**: goal in configuration space. 21 | - **base_pose**: pose for base of the robot or arm. 22 | - **dh_a**, **dh_alpha**, **dh_d**, **dh_theta**: DH parameters of the robot. 23 | - **robot_idx** and **link_idx**: when planning for only part of the robot, for example, PR2's arm, spheres need to be matched correctly to the GPMP2 robot model. A mapping between the true index of the link on the robot (**robot_idx**) and its index in the GPMP2 model (**link_idx**) has to be provided. 24 | - **starttraj** (optional): initialization of the trajectory. Straight line used if not passed. 25 | - **total_step**: total states to be optimized on the trajectory. 26 | - **obs_check_inter**: number of states between any two optimized states where obstacle cost is evaluated. 27 | - **output_inter**: number of states between any two optimized states for the output trajectory. 28 | - **total_time**: Total runtime of the trajectory in seconds. 29 | - **fix_pose_sigma** and **fix_vel_sigma**: pose/velocity prior model covariance. 30 | - **cost_sigma**: \sigma_obs for obstacle cost that controls the balance between 'smoothness' of the trajectory and 'obstacle avoidance'. Smaller \sigma_obs refers to less smooth trajectory, with lower probability of colliding with obstacles and vice versa. 31 | - **hinge_loss_eps**: \epsilon for hinge loss function, the 'safety distance' from obstacles. 32 | - **Qc**: GP hyperparameter. -------------------------------------------------------------------------------- /examples/data/robots/pr2_gpmp2spheres.robot.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /examples/data/robots/pr2_chompspheres.robot.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /src/utils/grid_flood.c: -------------------------------------------------------------------------------- 1 | /** 2 | * \file grid_flood.c 3 | * \brief Implementation of cd_grid_flood, a flood-fill implementation. 4 | */ 5 | 6 | #include 7 | #include "grid.h" 8 | #include "grid_flood.h" 9 | 10 | int cd_grid_flood_fill(struct cd_grid * g, size_t index_start, 11 | int * wrap_dim, 12 | int (*replace)(void *, void *), void * rptr) 13 | { 14 | /* we maintain a stack of yet-to-be-visited adjacent cells */ 15 | struct stack 16 | { 17 | struct stack * next; 18 | size_t index; 19 | }; 20 | 21 | int ret; 22 | struct stack * s; 23 | struct stack * popped; 24 | size_t index; 25 | int ni; 26 | int pm; 27 | int * subs; 28 | int sub_save; 29 | 30 | subs = (int *) malloc(g->n * sizeof(int)); 31 | if (!subs) return -1; 32 | 33 | /* Start with a stack with one node */ 34 | s = (struct stack *) malloc(sizeof(struct stack)); 35 | if (!s) { free(subs); return -1; } 36 | s->next = 0; 37 | s->index = index_start; 38 | 39 | /* Flood fill! */ 40 | ret = 0; 41 | while (s) 42 | { 43 | /* Pop */ 44 | popped = s; 45 | s = s->next; 46 | index = popped->index; 47 | free(popped); 48 | 49 | /* Attempt replace (continue if failed) */ 50 | // If not replaced, continue 51 | if (!replace(cd_grid_get_index(g,index), rptr)) continue; 52 | 53 | cd_grid_index_to_subs(g, index, subs); 54 | 55 | /* Add neighbors to stack */ 56 | for (ni=0; nin; ni++) 57 | for (pm=0; pm<2; pm++) 58 | { 59 | sub_save = subs[ni]; 60 | subs[ni] += pm==0 ? -1 : 1; 61 | /* bail if over the edge and not wrapping this dim */ 62 | if ((subs[ni] < 0 || subs[ni] >= g->sizes[ni]) && (!wrap_dim || !wrap_dim[ni])) 63 | { 64 | subs[ni] = sub_save; 65 | continue; 66 | } 67 | /* wrap if necessary */ 68 | if (subs[ni] < 0) subs[ni] += g->sizes[ni]; 69 | if (subs[ni] >= g->sizes[ni]) subs[ni] -= g->sizes[ni]; 70 | cd_grid_index_from_subs(g, &index, subs); 71 | subs[ni] = sub_save; 72 | 73 | popped = (struct stack *) malloc(sizeof(struct stack)); 74 | if (!popped) { ret = -1; goto do_return; } 75 | popped->next = s; 76 | popped->index = index; 77 | s = popped; 78 | } 79 | } 80 | 81 | do_return: 82 | /* free any remaining stack elements */ 83 | while (s) 84 | { 85 | popped = s; 86 | s = s->next; 87 | free(popped); 88 | } 89 | 90 | free(subs); 91 | return ret; 92 | } -------------------------------------------------------------------------------- /examples/data/robots/barrettwam_gpmp2spheres.robot.xml: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | 0 0 0 7 | 0 0 0 0 8 | 9 | 10 | 11 | 12 | 13 | wam7 14 | 15 | 16 | wam7 17 | handbase 18 | 0 0 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | wam0 52 | wam7 53 | WAM7ikfast 0.05 54 | 0 0 0.22 55 | JF1 JF2 JF3 JF4 56 | 1 1 1 0 57 | 0 0 1 58 | 59 | -------------------------------------------------------------------------------- /src/utils/util_shparse.c: -------------------------------------------------------------------------------- 1 | /** 2 | * \file util_shparse.c 3 | * \brief Implementation of cd_util_shparse, a simple POSIX shell argument 4 | * parser. 5 | */ 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include "util_shparse.h" 13 | 14 | int cd_util_shparse(char * in, int * argcp, char *** argvp) 15 | { 16 | int inarg; 17 | char quot; 18 | int i; 19 | int skipped; 20 | int argc; 21 | int argi; 22 | char ** argv; 23 | 24 | /* first, figure out how many arguments there are */ 25 | argc = 0; 26 | inarg = 0; 27 | quot = 0; 28 | skipped = 0; 29 | for (i=0; in[i]; i++) 30 | { 31 | /* swallow spaces inter-arg */ 32 | if (!inarg) 33 | { 34 | if (isspace(in[i])) 35 | { 36 | in[i] = 0; 37 | continue; 38 | } 39 | inarg = 1; 40 | argc++; 41 | skipped = 0; 42 | } 43 | /* are we ending an argument? */ 44 | if (inarg && !quot && isspace(in[i])) 45 | { 46 | for (; skipped >= 0; skipped--) 47 | in[i-skipped] = 0; 48 | inarg = 0; 49 | continue; 50 | } 51 | /* are we entering / leaving quotes? */ 52 | if (inarg && !quot && (in[i] == '"' || in[i] == '\'')) 53 | { 54 | quot = in[i]; 55 | skipped++; 56 | continue; 57 | } 58 | if (inarg && quot && in[i] == quot) 59 | { 60 | quot = 0; 61 | skipped++; 62 | continue; 63 | } 64 | /* is this a backslash escape? */ 65 | if (inarg && (!quot || quot == '"') && in[i] == '\\' && in[i+1]) 66 | { 67 | if (in[i+1] == '\n') 68 | { 69 | i++; 70 | skipped += 2; 71 | continue; 72 | } 73 | if (!quot || in[i+1] == '"' || in[i+1] == '\\') 74 | { 75 | i++; 76 | skipped++; 77 | } 78 | } 79 | /* copy the character! */ 80 | in[i-skipped] = in[i]; 81 | } 82 | /* copy in necessary null at the end */ 83 | for (; skipped >= 0; skipped--) 84 | in[i-skipped] = 0; 85 | 86 | argv = (char **) malloc(argc * sizeof(char *)); 87 | if (!argv) return -1; 88 | 89 | inarg = 0; 90 | for (argi=0, i=0; argi 2 | 4 | 5 | 6 | 0 0 0 7 | 0 0 0 0 8 | 9 | 10 | 11 | 12 | 13 | wam7 14 | 15 | 16 | wam7 17 | handbase 18 | 0 0 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | wam0 54 | wam7 55 | WAM7ikfast 0.05 56 | 0 0 0.22 57 | JF1 JF2 JF3 JF4 58 | 1 1 1 0 59 | 0 0 1 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /examples/gpmp2_wam.py: -------------------------------------------------------------------------------- 1 | import atexit 2 | from openravepy import * 3 | import orgpmp2.orgpmp2 4 | import time 5 | import types 6 | import prpy.kin 7 | import prpy.rave 8 | import sys 9 | 10 | # Initialize openrave logging 11 | from openravepy.misc import InitOpenRAVELogging 12 | InitOpenRAVELogging() 13 | 14 | # Start and end joint angles 15 | start_joints = numpy.array([1.57,1.2,0,1.7,0,-1.24,1.57]) 16 | end_joints = numpy.array([-0.26,0.6,0,1.3,0,-0.25,1.57]) 17 | 18 | # Set up openrave and load env 19 | RaveInitialize(True, level=DebugLevel.Info) 20 | atexit.register(RaveDestroy) 21 | e = Environment() 22 | atexit.register(e.Destroy) 23 | e.Load('data/envs/lab.env.xml') 24 | e.Load('data/robots/barrettwam_gpmp2spheres.robot.xml') 25 | e.SetViewer('qtcoin') 26 | 27 | # Get the robot 28 | r = e.GetRobots()[0] 29 | raveLogInfo("Robot "+r.GetName()+" (#links: " + str(len(r.GetLinks())) + ") has "+repr(r.GetDOF())+" joints with values:\n"+repr(r.GetDOFValues())) 30 | 31 | # Set robot joint active 32 | r.SetActiveManipulator('arm') 33 | m = r.GetActiveManipulator() 34 | r.SetActiveDOFs(m.GetArmIndices()) 35 | r.SetActiveDOFValues(start_joints) 36 | 37 | # Load gpmp2 38 | m_gpmp2 = RaveCreateModule(e,'orgpmp2') 39 | orgpmp2.orgpmp2.bind(m_gpmp2) 40 | 41 | # Compute distance field for the env 42 | # first disable robot 43 | r.Enable(False) 44 | # get a distance field for env 45 | m_gpmp2.computedistancefield(cache_filename='sdf_env_lab.dat', 46 | centroid=numpy.array([0,0,0]),extents=numpy.array([1.2,1.2,1.2]),res=0.02) 47 | # enable robot 48 | r.Enable(True) 49 | 50 | # DH parameters of robot 51 | alpha = numpy.array([-1.5708, 1.5708, -1.5708, 1.5708, -1.5708, 1.5708, 0]) 52 | a = numpy.array([0, 0, 0.045, -0.045, 0, 0, 0]) 53 | d = numpy.array([0, 0, 0.55, 0, 0.3, 0, 0.06]) 54 | theta = numpy.array([0, 0, 0, 0, 0, 0, 0]) 55 | 56 | # Run gpmp2 57 | try: 58 | t = m_gpmp2.rungpmp2( 59 | robot = r, 60 | end_conf = end_joints, 61 | dh_a = a, 62 | dh_alpha = alpha, 63 | dh_d = d, 64 | dh_theta = theta, 65 | total_step = 10, 66 | obs_check_inter = 9, 67 | output_inter = 9, 68 | total_time = 1.0, 69 | fix_pose_sigma = 1e-3, 70 | fix_vel_sigma = 1e-3, 71 | cost_sigma = 0.02, 72 | hinge_loss_eps = 0.2, 73 | Qc = 1, 74 | no_report_cost = False, 75 | no_collision_exception = True) 76 | except RuntimeError as ex: 77 | print ex 78 | t = None 79 | 80 | # Load objects 81 | e.Load('data/envs/objects.xml') 82 | r.SetActiveDOFValues(start_joints) 83 | raw_input('Press [Enter] to grasp object ...') 84 | tp = interfaces.TaskManipulation(r) 85 | tp.CloseFingers() 86 | r.Grab(e.GetKinBody('soda')) 87 | 88 | # Run optimized trajectory 89 | try: 90 | while t is not None: 91 | raw_input('Press [Enter] to run the trajectory ...\n') 92 | with e: 93 | r.GetController().SetPath(t) 94 | except KeyboardInterrupt: 95 | print -------------------------------------------------------------------------------- /doc/Usage.md: -------------------------------------------------------------------------------- 1 | Usage 2 | ============= 3 | Before using the python script make sure all the dependencies and the plugin are installed correctly. The example python scripts are located in the [examples](../examples) folder and the robot and environment files are located in the [data](../examples/data) folder. WAM is setup for the lab environment and PR2 works with several environments. For convenience, settings based on environments for PR2 are accessed through [problemsets.py](../examples/data/problemsets.py). 4 | 5 | 6 | Initialize module 7 | ----- 8 | Once openrave is initialized with appropriate environment and robot xml files set the active DOF of the robot and their **start_conf**. Then create an object of orgpmp2 9 | 10 | ```python 11 | m_gpmp2 = RaveCreateModule(e,'orgpmp2') 12 | orgpmp2.orgpmp2.bind(m_gpmp2) 13 | ``` 14 | 15 | 16 | Signed Distance Field 17 | ----- 18 | Signed distance field is calculated by the ```computedistancefield``` function. First disable the part of the robot being planned for and then compute the SDF. For example, [WAM](../examples/gpmp2_wam.py) is disabled completely but [PR2 example](../examples/gpmp2_pr2.py) plans for it's right arm so only that is disabled. 19 | 20 | ```python 21 | # Compute distance field for the env 22 | # first disable robot 23 | r.Enable(False) 24 | # get a distance field for env 25 | m_gpmp2.computedistancefield(cache_filename='sdf_env_lab.dat', 26 | centroid=numpy.array([0,0,0]),extents=numpy.array([1.2,1.2,1.2]),res=0.02) 27 | # enable robot 28 | r.Enable(True) 29 | ``` 30 | See [Parameters](Parameters.md) for their description and how to set them. 31 | 32 | 33 | Initializing trajectory 34 | ----- 35 | **starttraj** is used to pass an initialization of the trajectory if available. If nothing is passed a straight line initialization is used by default. The trajectory can be initialized as follows 36 | 37 | ```python 38 | st = RaveCreateTrajectory(e,'') 39 | st.Init(r.GetActiveConfigurationSpecification()) 40 | init_traj = numpy.append(init_p,init_v) # (position, velocity) 41 | st.Insert(0,init_traj) 42 | ``` 43 | See initializing with trajectory [example](../examples/gpmp2_wam_withtraj.py) on WAM for more details. 44 | 45 | 46 | Running GPMP2 47 | ----- 48 | After setting up the environment and calculating the SDF the GPMP2 algorithm can be run 49 | 50 | ```python 51 | # Run gpmp2 52 | try: 53 | t = m_gpmp2.rungpmp2(...parameters go here...) 54 | except RuntimeError as ex: 55 | print ex 56 | t = None 57 | ``` 58 | See [Parameters](Parameters.md) for their description and how to set them. 59 | 60 | 61 | Displaying optimized trajectory 62 | ----- 63 | Once a solution is found it can be displayed as follows 64 | 65 | ```python 66 | # Run optimized trajectory 67 | try: 68 | while t is not None: 69 | raw_input('Press [Enter] to run the trajectory ...\n') 70 | with e: 71 | r.GetController().SetPath(t) 72 | except KeyboardInterrupt: 73 | print 74 | ``` -------------------------------------------------------------------------------- /src/utils/grid.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file grid.h 3 | * \brief Interface to cd_grid, a multidimensional grid of values. 4 | */ 5 | 6 | /* requires: 7 | * #include (for size_t) 8 | */ 9 | 10 | struct cd_grid 11 | { 12 | /* Dimensionality of space */ 13 | int n; 14 | /* Grid parameters */ 15 | int * sizes; 16 | size_t ncells; 17 | /* The actual data */ 18 | int cell_size; 19 | char * data; 20 | /* Actual grid side lengths (1x1x1x... by default) */ 21 | double * lengths; 22 | }; 23 | 24 | int cd_grid_create(struct cd_grid ** gp, void * cell_init, int cell_size, int n, ...); 25 | int cd_grid_create_sizearray(struct cd_grid ** gp, void * cell_init, int cell_size, int n, int * sizes); 26 | 27 | /* note - this takes ownership of sizes, and will free it when the grid is destroyed! */ 28 | int cd_grid_create_sizeown(struct cd_grid ** gp, void * cell_init, int cell_size, int n, int * sizes); 29 | 30 | int cd_grid_create_copy(struct cd_grid ** gp, struct cd_grid * gsrc); 31 | int cd_grid_destroy(struct cd_grid * g); 32 | 33 | /* convert between index and subs */ 34 | int cd_grid_index_to_subs(struct cd_grid * g, size_t index, int * subs); 35 | int cd_grid_index_from_subs(struct cd_grid * g, size_t * index, int * subs); 36 | 37 | /* Get center from index */ 38 | int cd_grid_center_index(struct cd_grid * g, size_t index, double * center); 39 | 40 | /* Lookup cell index, subs from location 41 | * these will return 1 if it's out of range! */ 42 | int cd_grid_lookup_index(struct cd_grid * g, double * p, size_t * index); 43 | int cd_grid_lookup_subs(struct cd_grid * g, double * p, int * subs); 44 | 45 | 46 | /* Get cell from index, subs */ 47 | void * cd_grid_get_index(struct cd_grid * g, size_t index); 48 | void * cd_grid_get_subs(struct cd_grid * g, int * subs); 49 | void * cd_grid_get(struct cd_grid * g, ...); 50 | 51 | 52 | /* Simple multilinear interpolation / discontinuous gradient 53 | * these will return 1 if the point is out of range! */ 54 | int cd_grid_double_interp(struct cd_grid * g, double * p, double * valuep); 55 | int cd_grid_double_grad(struct cd_grid * g, double * p, double * grad); 56 | 57 | 58 | 59 | /* Squared Euclidean Distance Transform 60 | * (see http://www.cs.cornell.edu/~dph/papers/dt.pdf) 61 | * Uses doubles! 62 | * Given a double grid funcg, 63 | * computes the sedt grid (same size), 64 | * with 0 -> 0, and + -> squared distance to smallest value */ 65 | int cd_grid_double_dt_sqeuc(struct cd_grid ** gp_dt, struct cd_grid * g_func); 66 | /* legacy */ 67 | int cd_grid_double_sedt(struct cd_grid ** gp_dt, struct cd_grid * g_func); 68 | 69 | /* signed euclidean distance transform, aka signed distance field 70 | * g_binobs is type(char) with 0 in free space, !=0 in obstacles 71 | * output is + inside obstacles, - in free space */ 72 | int cd_grid_double_dt_sgneuc(struct cd_grid ** gp_dt, struct cd_grid * g_binobs); 73 | /* legacy (g_emp is HUGE_VAL in obstacles, and 0.0 in free space) */ 74 | int cd_grid_double_bin_sdf(struct cd_grid ** gp_dt, struct cd_grid * g_emp); -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | orgpmp2 2 | =================================================== 3 | orgpmp2 is a OpenRAVE plugin for GPMP2 (Gaussian Process Motion Planner 2) algorithm described in [Motion Planning as Probabilistic Inference using Gaussian Processes and Factor Graphs](http://www.cc.gatech.edu/~bboots3/files/GPMP2.pdf) (RSS 2016). Examples provided for WAM arm and PR2 use python scripts. 4 | 5 | orgpmp2 is being developed by [Mustafa Mukadam](mailto:mmukadam3@gatech.edu) and [Jing Dong](mailto:thu.dongjing@gmail.com) as part of their work at Georgia Tech Robot Learning Lab and is a modified version of [CHOMP openrave plugin](https://github.com/personalrobotics/or_cdchomp). 6 | 7 | Compilation & Installation 8 | ------ 9 | 10 | - Install [ROS](http://wiki.ros.org/indigo/Installation/Ubuntu). We have tested ROS indigo. 11 | - Install [OpenRAVE](http://openrave.org/). If you have trouble compiling the original version, please check out and install [our fork](https://github.com/gtrll/openrave) which has fixed few minor bugs. 12 | - Install [GPMP2](https://github.com/gtrll/gpmp2) core C++ library. 13 | - Install a few additional dependencies 14 | 15 | ```bash 16 | sudo apt-get install gfortran libgsl0-dev python-enum 17 | ``` 18 | 19 | - Initialize a catkin workspace (if you use a existing catkin workspace this step is not needed) 20 | 21 | ```bash 22 | mkdir -p ~/gpmp2_catkin_ws/src 23 | cd gpmp2_catkin_ws/src 24 | catkin_init_workspace 25 | ``` 26 | 27 | - Add orgpmp2, [PrPy](https://github.com/personalrobotics/prpy) (python library for OpenRAVE), [openrave_catkin](https://github.com/personalrobotics/openrave_catkin) (utility package for OpenRAVE) to ```gpmp2_catkin_ws/src``` 28 | 29 | ```bash 30 | git clone https://github.com/gtrll/orgpmp2.git 31 | git clone https://github.com/personalrobotics/openrave_catkin.git 32 | git clone https://github.com/personalrobotics/prpy.git 33 | ``` 34 | 35 | - Compile the catkin workspace 36 | 37 | ```bash 38 | catkin_make -DCMAKE_BUILD_TYPE=Release 39 | ``` 40 | 41 | - Before running the examples, the last step is setup the environment variables 42 | 43 | ```bash 44 | source ~/gpmp2_catkin_ws/devel/setup.bash 45 | ``` 46 | 47 | Questions & Bug reporting 48 | ----- 49 | 50 | Please use Github issue tracker to report bugs. For other questions please contact [Mustafa Mukadam](mailto:mmukadam3@gatech.edu) or [Jing Dong](mailto:thu.dongjing@gmail.com). 51 | 52 | 53 | Citing 54 | ----- 55 | 56 | If you use orgpmp2 or GPMP2 in an academic context, please cite following publications: 57 | 58 | ``` 59 | @inproceedings{Dong-RSS-16, 60 | Author = "Jing Dong and Mustafa Mukadam and Frank Dellaert and Byron Boots", 61 | booktitle = {Proceedings of Robotics: Science and Systems (RSS-2016)}, 62 | Title = "Motion Planning as Probabilistic Inference using Gaussian Processes and Factor Graphs", 63 | year = {2016} 64 | } 65 | ``` 66 | 67 | License 68 | ----- 69 | 70 | orgpmp2 is released under the GPL license, reproduced in the file license-gpl.txt in this directory. 71 | -------------------------------------------------------------------------------- /examples/save_sdf_pr2.py: -------------------------------------------------------------------------------- 1 | import atexit 2 | from openravepy import * 3 | import orgpmp2.orgpmp2 4 | import time 5 | import types 6 | import prpy.kin 7 | import prpy.rave 8 | import sys 9 | sys.path.append('data') 10 | import problemsets 11 | 12 | # Initialize openrave logging 13 | from openravepy.misc import InitOpenRAVELogging 14 | InitOpenRAVELogging() 15 | 16 | # Problemset 17 | problemset = 'industrial2' 18 | 19 | # Start and end joint angles 20 | n_states, states = problemsets.states(problemset) 21 | start_joints = numpy.array(states[3]) 22 | end_joints = numpy.array(states[1]) 23 | 24 | # Set up openrave and load env 25 | RaveInitialize(True, level=DebugLevel.Info) 26 | atexit.register(RaveDestroy) 27 | e = Environment() 28 | atexit.register(e.Destroy) 29 | e.Load(problemsets.env_file(problemset)) 30 | e.Load('data/robots/pr2_gpmp2spheres.robot.xml') 31 | e.SetViewer('qtcoin') 32 | 33 | # Set up robot 34 | r = e.GetRobots()[0] 35 | r.SetTransform(matrixFromPose(problemsets.default_base_pose(problemset))) 36 | rave_joint_names = [joint.GetName() for joint in r.GetJoints()] 37 | rave_inds, rave_values = [],[] 38 | for (name,val) in zip(problemsets.joint_names(problemset), problemsets.default_joint_values(problemset)): 39 | if name in rave_joint_names: 40 | i = rave_joint_names.index(name) 41 | rave_inds.append(i) 42 | rave_values.append(val) 43 | r.SetDOFValues(rave_values, rave_inds) 44 | active_joint_inds = [rave_joint_names.index(name) for name in problemsets.active_joints(problemset)] 45 | r.SetActiveDOFs(active_joint_inds, problemsets.active_affine(problemset)) 46 | r.SetActiveDOFValues(start_joints) 47 | 48 | # Calculate arm_pose 49 | l = r.GetLinks() 50 | for i in l: 51 | if (i.GetName() == "torso_lift_link"): 52 | lp1 = poseFromMatrix(i.GetTransform()) 53 | if (i.GetName() == "r_shoulder_pan_link"): 54 | lp2 = poseFromMatrix(i.GetTransform()) 55 | arm_pose = numpy.array([lp1[0], lp1[1], lp1[2], lp1[3], lp2[4], lp2[5], lp2[6]]) 56 | arm_origin = numpy.array([lp2[4], lp2[5], lp2[6]]) 57 | 58 | # Load gpmp2 59 | m_gpmp2 = RaveCreateModule(e,'orgpmp2') 60 | orgpmp2.orgpmp2.bind(m_gpmp2) 61 | 62 | # SDF 63 | # remove right arm links to calculate sdf 64 | l = r.GetLinks() 65 | right_arm_links = ['r_shoulder_pan_link', 'r_shoulder_lift_link', 66 | 'r_upper_arm_roll_link', 'r_upper_arm_link', 'r_elbow_flex_link', 'r_forearm_roll_link', 67 | 'r_forearm_cam_frame', 'r_forearm_cam_optical_frame', 'r_forearm_link', 'r_wrist_flex_link', 68 | 'r_wrist_roll_link', 'r_gripper_palm_link', 'r_gripper_l_finger_link', 'r_gripper_l_finger_tip_link', 69 | 'r_gripper_motor_slider_link', 'r_gripper_motor_screw_link', 'r_gripper_led_frame', 70 | 'r_gripper_motor_accelerometer_link', 'r_gripper_r_finger_link', 'r_gripper_r_finger_tip_link', 71 | 'r_gripper_l_finger_tip_frame', 'r_gripper_tool_frame'] 72 | for i in l: 73 | if i.GetName() in right_arm_links: 74 | i.Enable(False) 75 | # Compute distance field for the env and remaining robot 76 | m_gpmp2.computedistancefield(cache_filename='sdf_env_'+problemset+'.dat', 77 | centroid=arm_origin,extents=numpy.array([1.2,1.2,1.2]),res=0.02,save_sdf=1) 78 | 79 | raw_input() 80 | sys.exit() 81 | -------------------------------------------------------------------------------- /examples/gpmp2_wam_withtraj.py: -------------------------------------------------------------------------------- 1 | import atexit 2 | from openravepy import * 3 | import orgpmp2.orgpmp2 4 | import time 5 | import types 6 | import prpy.kin 7 | import prpy.rave 8 | import sys 9 | 10 | # Initialize openrave logging 11 | from openravepy.misc import InitOpenRAVELogging 12 | InitOpenRAVELogging() 13 | 14 | # Start and end joint angles 15 | start_joints = numpy.array([1.57,1.2,0,1.7,0,-1.24,1.57]) 16 | end_joints = numpy.array([-0.26,0.6,0,1.3,0,-0.25,1.57]) 17 | 18 | # Set up openrave and load env 19 | RaveInitialize(True, level=DebugLevel.Info) 20 | atexit.register(RaveDestroy) 21 | e = Environment() 22 | atexit.register(e.Destroy) 23 | e.Load('data/envs/lab.env.xml') 24 | e.Load('data/robots/barrettwam_gpmp2spheres.robot.xml') 25 | e.SetViewer('qtcoin') 26 | 27 | # Get the robot 28 | r = e.GetRobots()[0] 29 | raveLogInfo("Robot "+r.GetName()+" (#links: " + str(len(r.GetLinks())) + ") has "+repr(r.GetDOF())+" joints with values:\n"+repr(r.GetDOFValues())) 30 | 31 | # Set robot joint active 32 | r.SetActiveManipulator('arm') 33 | m = r.GetActiveManipulator() 34 | r.SetActiveDOFs(m.GetArmIndices()) 35 | r.SetActiveDOFValues(start_joints) 36 | 37 | # Load gpmp2 38 | m_gpmp2 = RaveCreateModule(e,'orgpmp2') 39 | orgpmp2.orgpmp2.bind(m_gpmp2) 40 | 41 | # Compute distance field for the env 42 | # first disable robot 43 | r.Enable(False) 44 | # get a distance field for env 45 | m_gpmp2.computedistancefield(cache_filename='sdf_env_lab.dat', 46 | centroid=numpy.array([0,0,0]),extents=numpy.array([1.2,1.2,1.2]),res=0.02) 47 | # enable robot 48 | r.Enable(True) 49 | 50 | # DH parameters of robot 51 | alpha = numpy.array([-1.5708, 1.5708, -1.5708, 1.5708, -1.5708, 1.5708, 0]) 52 | a = numpy.array([0, 0, 0.045, -0.045, 0, 0, 0]) 53 | d = numpy.array([0, 0, 0.55, 0, 0.3, 0, 0.06]) 54 | theta = numpy.array([0, 0, 0, 0, 0, 0, 0]) 55 | 56 | # Initialize Trajectory 57 | total_step = 10 58 | st = RaveCreateTrajectory(e,'') 59 | st.Init(r.GetActiveConfigurationSpecification()) 60 | zero = numpy.zeros(len(start_joints)) 61 | init_p = [] 62 | init_v = [] 63 | for i in range(total_step): 64 | mid = (1-i/((total_step-1)*1.0))*start_joints + i/((total_step-1)*1.0)*end_joints 65 | init_p = numpy.append(init_p,mid) 66 | init_v = numpy.append(init_v,zero) 67 | init_traj = numpy.append(init_p,init_v) 68 | st.Insert(0,init_traj) 69 | 70 | # Run gpmp2 71 | try: 72 | t = m_gpmp2.rungpmp2( 73 | robot = r, 74 | end_conf = end_joints, 75 | dh_a = a, 76 | dh_alpha = alpha, 77 | dh_d = d, 78 | dh_theta = theta, 79 | starttraj=st, 80 | total_step = total_step, 81 | obs_check_inter = 9, 82 | output_inter = 9, 83 | total_time = 1.0, 84 | fix_pose_sigma = 1e-3, 85 | fix_vel_sigma = 1e-3, 86 | cost_sigma = 0.02, 87 | hinge_loss_eps = 0.2, 88 | Qc = 1, 89 | no_report_cost = False, 90 | no_collision_exception = True) 91 | except RuntimeError as ex: 92 | print ex 93 | t = None 94 | 95 | # Run optimized trajectory 96 | try: 97 | while t is not None: 98 | raw_input('Press [Enter] to run the trajectory ...\n') 99 | with e: 100 | r.GetController().SetPath(t) 101 | except KeyboardInterrupt: 102 | print -------------------------------------------------------------------------------- /src/orgpmp2_kdata.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file orgpmp2_kdata.cpp 3 | * \brief Implementation of orgpmp2_rdata, a parser for sphere data 4 | * provided with an OpenRAVE kinbody XML file. 5 | */ 6 | 7 | #include 8 | 9 | #include "orgpmp2_kdata.h" 10 | 11 | namespace orgpmp2 12 | { 13 | 14 | kdata::kdata() : OpenRAVE::XMLReadable("orgpmp2") 15 | { 16 | this->sphereelems = 0; 17 | } 18 | 19 | kdata::~kdata() 20 | { 21 | struct sphereelem * e; 22 | while (this->sphereelems) 23 | { 24 | e=this->sphereelems->next; 25 | free(this->sphereelems->s); 26 | free(this->sphereelems); 27 | this->sphereelems = e; 28 | } 29 | } 30 | 31 | 32 | kdata_parser::kdata_parser(boost::shared_ptr passed_d, const OpenRAVE::AttributesList& atts) 33 | { 34 | /* save or construct the kdata object */ 35 | this->d = passed_d; 36 | if(!this->d) this->d.reset(new kdata()); 37 | /* get ready */ 38 | this->inside_spheres = false; 39 | } 40 | 41 | OpenRAVE::XMLReadablePtr kdata_parser::GetReadable() 42 | { 43 | return this->d; 44 | } 45 | 46 | OpenRAVE::BaseXMLReader::ProcessElement kdata_parser::startElement(const std::string& name, const OpenRAVE::AttributesList& atts) 47 | { 48 | if (name == "spheres") 49 | { 50 | if (this->inside_spheres) RAVELOG_ERROR("you can't have inside !\n"); 51 | this->inside_spheres = true; 52 | return PE_Support; 53 | } 54 | if (name == "sphere") 55 | { 56 | struct sphere * s; 57 | struct sphereelem * e; 58 | if (!this->inside_spheres) { RAVELOG_ERROR("you can't have not inside !\n"); return PE_Pass; } 59 | s = (struct sphere *) malloc(sizeof(struct sphere)); 60 | for(OpenRAVE::AttributesList::const_iterator itatt = atts.begin(); itatt != atts.end(); ++itatt) 61 | { 62 | if (itatt->first=="link") 63 | strcpy(s->linkname, itatt->second.c_str()); 64 | else if (itatt->first=="radius") 65 | s->radius = strtod(itatt->second.c_str(), 0); 66 | else if (itatt->first=="pos") 67 | sscanf(itatt->second.c_str(), "%lf %lf %lf", &s->pos[0], &s->pos[1], &s->pos[2]); 68 | else 69 | RAVELOG_ERROR("unknown attribute %s=%s!\n",itatt->first.c_str(),itatt->second.c_str()); 70 | } 71 | /* insert at head of kdata list */ 72 | e = (struct sphereelem *) malloc(sizeof(struct sphereelem)); 73 | e->s = s; 74 | e->next = this->d->sphereelems; 75 | this->d->sphereelems = e; 76 | return PE_Support; 77 | } 78 | return PE_Pass; 79 | } 80 | 81 | void kdata_parser::characters(const std::string& ch) 82 | { 83 | return; 84 | } 85 | 86 | bool kdata_parser::endElement(const std::string& name) 87 | { 88 | if (name == "orgpmp2") return true; 89 | if (name == "spheres") 90 | { 91 | if (!this->inside_spheres) RAVELOG_ERROR("you can't have without matching !\n"); 92 | this->inside_spheres = false; 93 | } 94 | else if (name == "sphere") 95 | { 96 | if (!this->inside_spheres) RAVELOG_ERROR("you can't have not inside !\n"); 97 | } 98 | else 99 | RAVELOG_ERROR("unknown field %s\n", name.c_str()); 100 | return false; 101 | } 102 | 103 | } /* namespace orgpmp2 */ -------------------------------------------------------------------------------- /src/utils/kin.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file kin.h 3 | * \brief Interface to cd_kin, a collection of useful routines for 4 | * kinematics. 5 | */ 6 | 7 | #ifndef CD_KIN_H 8 | #define CD_KIN_H 9 | 10 | 11 | int cd_kin_quat_identity(double quat[4]); 12 | int cd_kin_pose_identity(double pose[7]); 13 | 14 | int cd_kin_quat_normalize(double quat[4]); 15 | int cd_kin_pose_normalize(double pose[7]); 16 | 17 | int cd_kin_quat_flip_closerto(double quat[4], const double target[4]); 18 | int cd_kin_pose_flip_closerto(double pose[7], const double target[7]); 19 | 20 | 21 | /* composition */ 22 | /* qab * qbc = qac 23 | * aka Hamilton product; 24 | * it's OK if output is either of the inputs */ 25 | int cd_kin_quat_compose(const double quat_ab[4], const double quat_bc[4], double quat_ac[4]); 26 | int cd_kin_pose_compose(const double pose_ab[7], const double pose_bc[7], double pose_ac[7]); 27 | 28 | /* Do the same, but for a 3d pos, not a full pose */ 29 | int cd_kin_pose_compos(const double pose_ab[7], const double pos_bc[3], double pos_ac[3]); 30 | 31 | /* this is rotation, vac = q vbc q* */ 32 | int cd_kin_quat_compose_vec(const double quat_ab[4], const double vec_bc[3], double vec_ac[3]); 33 | /* Do the same, but just rotate a 3d vector (vel,acc), not a position vector */ 34 | int cd_kin_pose_compose_vec(const double pose_ab[7], const double vec_bc[3], double vec_ac[3]); 35 | 36 | /* note conf equals inverse for unit quaternions */ 37 | int cd_kin_quat_invert(const double quat_in[4], double quat_out[4]); 38 | int cd_kin_pose_invert(const double pose_in[7], double pose_out[7]); 39 | 40 | 41 | /* conversion to/from rotation matrices */ 42 | int cd_kin_quat_to_R(const double quat[4], double R[3][3]); 43 | int cd_kin_pose_to_H(const double pose[7], double H[4][4], int fill_bottom); 44 | int cd_kin_pose_to_dR(const double pose[7], double d[3], double R[3][3]); 45 | 46 | int cd_kin_quat_from_R(double quat[4], double R[3][3]); 47 | int cd_kin_pose_from_H(double pose[7], double H[3][4]); 48 | int cd_kin_pose_from_dR(double pose[7], const double d[3], double R[3][3]); 49 | 50 | 51 | /* conversion to/from axis-angle */ 52 | int cd_kin_quat_to_axisangle(const double quat[4], double axis[3], double *angle); 53 | int cd_kin_quat_from_axisangle(double quat[4], const double axis[3], const double angle); 54 | 55 | /* this is the equivalent of cd_kin_quat_compose_vecs above */ 56 | int cd_kin_axisangle_rotate(const double axis[3], const double angle, 57 | const double pos_in[3], double pos_out[3]); 58 | 59 | int cd_kin_axisangle_to_R(const double axis[3], const double angle, double R[3][3]); 60 | 61 | 62 | /* conversion to/from yaw-pitch-roll airplane euler angles */ 63 | int cd_kin_quat_to_ypr(const double quat[4], double ypr[3]); 64 | int cd_kin_pose_to_xyzypr(const double pose[7], double xyzypr[6]); 65 | 66 | int cd_kin_quat_to_ypr_J(const double quat[4], double J[3][4]); 67 | int cd_kin_pose_to_xyzypr_J(const double pose[7], double J[6][7]); 68 | 69 | int cd_kin_quat_from_ypr(double quat[4], const double ypr[3]); 70 | int cd_kin_pose_from_xyzypr(double pose[7], const double xyzypr[6]); 71 | 72 | 73 | /* convert between pose and pos+quat */ 74 | int cd_kin_pose_to_pos_quat(const double pose[7], double pos[3], double quat[4]); 75 | int cd_kin_pose_from_pos_quat(double pose[7], const double pos[3], const double quat[4]); 76 | 77 | 78 | /* get an arbitrary pose from specification */ 79 | int cd_kin_pose_from_op(double pose[7], const double from[3], const double to[3], double * const lenp); 80 | int cd_kin_pose_from_op_diff(double pose[7], const double from[3], const double to_diff[3], double * const lenp); 81 | 82 | #endif /* CD_KIN_H */ -------------------------------------------------------------------------------- /examples/data/envs/industrial.env.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0.2 0.7 0.01 6 | 0.75 -0.1 0.7 7 | 1 0 0 0 8 | 9 | 10 | 11 | 12 | 13 | 14 | 0.55 0.15 0.01 15 | 0.4 -0.65 0.7 16 | 1 0 0 0 17 | 18 | 19 | 20 | 21 | 22 | 23 | 0.01 0.1 0.15 24 | 0.1 -0.7 0.85 25 | 1 0 0 0 26 | 27 | 28 | 29 | 30 | 31 | 32 | 0.01 0.1 0.15 33 | 0.3 -0.7 0.85 34 | 1 0 0 0 35 | 36 | 37 | 38 | 39 | 40 | 41 | 0.01 0.1 0.15 42 | 0.5 -0.7 0.85 43 | 1 0 0 0 44 | 45 | 46 | 47 | 48 | 49 | 50 | 0.21 0.15 0.01 51 | 0.3 -0.65 0.71 52 | 1 0 0 0 53 | 54 | 55 | 56 | 57 | 58 | 59 | 0.01 0.05 0.03 60 | 0.1 -0.55 0.73 61 | 1 0 0 0 62 | 63 | 64 | 65 | 66 | 67 | 68 | 0.01 0.05 0.03 69 | 0.3 -0.55 0.73 70 | 1 0 0 0 71 | 72 | 73 | 74 | 75 | 76 | 77 | 0.01 0.05 0.03 78 | 0.5 -0.55 0.73 79 | 1 0 0 0 80 | 81 | 82 | 83 | 84 | 85 | 86 | 0.21 0.01 0.03 87 | 0.3 -0.5 0.73 88 | 1 0 0 0 89 | 90 | 91 | 92 | 93 | 94 | 95 | 0.21 0.01 0.03 96 | 0.3 -0.59 0.93 97 | 1 0 0 0 98 | 99 | 100 | 101 | 102 | 103 | 104 | 0.21 0.01 0.17 105 | 0.3 -0.79 0.87 106 | 1 0 0 0 107 | 108 | 109 | 110 | 111 | 112 | 113 | 0.21 0.1 0.01 114 | 0.3 -0.7 0.9 115 | 1 0 0 0 116 | 117 | 118 | 119 | 120 | 121 | 122 | 0.15 0.01 0.225 123 | 0.8 0.25 0.925 124 | 1 0 0 0 125 | 126 | 127 | 128 | 129 | 130 | 131 | 0.15 0.01 0.225 132 | 0.8 -0.25 0.925 133 | 1 0 0 0 134 | 135 | 136 | 137 | 138 | 139 | 140 | 0.15 0.26 0.01 141 | 0.8 -0.00 1.16 142 | 1 0 0 0 143 | 144 | 145 | 146 | -------------------------------------------------------------------------------- /examples/gpmp2_pr2.py: -------------------------------------------------------------------------------- 1 | import atexit 2 | from openravepy import * 3 | import orgpmp2.orgpmp2 4 | import time 5 | import types 6 | import prpy.kin 7 | import prpy.rave 8 | import sys 9 | sys.path.append('data') 10 | import problemsets 11 | 12 | # Initialize openrave logging 13 | from openravepy.misc import InitOpenRAVELogging 14 | InitOpenRAVELogging() 15 | 16 | # Problemset 17 | problemset = 'industrial2' 18 | 19 | # Start and end joint angles 20 | n_states, states = problemsets.states(problemset) 21 | start_joints = numpy.array(states[3]) 22 | end_joints = numpy.array(states[1]) 23 | 24 | # Set up openrave and load env 25 | RaveInitialize(True, level=DebugLevel.Info) 26 | atexit.register(RaveDestroy) 27 | e = Environment() 28 | atexit.register(e.Destroy) 29 | e.Load(problemsets.env_file(problemset)) 30 | e.Load('data/robots/pr2_gpmp2spheres.robot.xml') 31 | e.SetViewer('qtcoin') 32 | 33 | # Set up robot 34 | r = e.GetRobots()[0] 35 | r.SetTransform(matrixFromPose(problemsets.default_base_pose(problemset))) 36 | rave_joint_names = [joint.GetName() for joint in r.GetJoints()] 37 | rave_inds, rave_values = [],[] 38 | for (name,val) in zip(problemsets.joint_names(problemset), problemsets.default_joint_values(problemset)): 39 | if name in rave_joint_names: 40 | i = rave_joint_names.index(name) 41 | rave_inds.append(i) 42 | rave_values.append(val) 43 | r.SetDOFValues(rave_values, rave_inds) 44 | active_joint_inds = [rave_joint_names.index(name) for name in problemsets.active_joints(problemset)] 45 | r.SetActiveDOFs(active_joint_inds, problemsets.active_affine(problemset)) 46 | r.SetActiveDOFValues(start_joints) 47 | 48 | # Calculate arm_pose 49 | l = r.GetLinks() 50 | for i in l: 51 | if (i.GetName() == "torso_lift_link"): 52 | lp1 = poseFromMatrix(i.GetTransform()) 53 | if (i.GetName() == "r_shoulder_pan_link"): 54 | lp2 = poseFromMatrix(i.GetTransform()) 55 | arm_pose = numpy.array([lp1[0], lp1[1], lp1[2], lp1[3], lp2[4], lp2[5], lp2[6]]) 56 | arm_origin = numpy.array([lp2[4], lp2[5], lp2[6]]) 57 | 58 | # Load gpmp2 59 | m_gpmp2 = RaveCreateModule(e,'orgpmp2') 60 | orgpmp2.orgpmp2.bind(m_gpmp2) 61 | 62 | # SDF 63 | # remove right arm links to calculate sdf 64 | l = r.GetLinks() 65 | right_arm_links = ['r_shoulder_pan_link', 'r_shoulder_lift_link', 66 | 'r_upper_arm_roll_link', 'r_upper_arm_link', 'r_elbow_flex_link', 'r_forearm_roll_link', 67 | 'r_forearm_cam_frame', 'r_forearm_cam_optical_frame', 'r_forearm_link', 'r_wrist_flex_link', 68 | 'r_wrist_roll_link', 'r_gripper_palm_link', 'r_gripper_l_finger_link', 'r_gripper_l_finger_tip_link', 69 | 'r_gripper_motor_slider_link', 'r_gripper_motor_screw_link', 'r_gripper_led_frame', 70 | 'r_gripper_motor_accelerometer_link', 'r_gripper_r_finger_link', 'r_gripper_r_finger_tip_link', 71 | 'r_gripper_l_finger_tip_frame', 'r_gripper_tool_frame'] 72 | for i in l: 73 | if i.GetName() in right_arm_links: 74 | i.Enable(False) 75 | # Compute distance field for the env and remaining robot 76 | m_gpmp2.computedistancefield(cache_filename='sdf_env_'+problemset+'.dat', 77 | centroid=arm_origin,extents=numpy.array([1.2,1.2,1.2]),res=0.02) 78 | # enable robot 79 | r.Enable(True) 80 | 81 | # DH parameters of robot 82 | alpha = numpy.array([-1.5708, 1.5708, -1.5708, 1.5708, -1.5708, 1.5708, 0]) 83 | a = numpy.array([0.1, 0, 0, 0, 0, 0, 0]) 84 | d = numpy.array([0, 0, 0.4, 0, 0.321, 0, 0]) 85 | theta = numpy.array([0, 1.5708, 0, 0, 0, 0, 0]) 86 | # robot id (full robot in openrave) to link id (just arm for gpmp2) mapping 87 | robot_idx = numpy.array([60, 62, 65, 70]) 88 | link_idx = numpy.array([0, 2, 4, 6]) 89 | 90 | # Run gpmp2 91 | try: 92 | t = m_gpmp2.rungpmp2( 93 | robot = r, 94 | end_conf = end_joints, 95 | base_pose = arm_pose, 96 | dh_a = a, 97 | dh_alpha = alpha, 98 | dh_d = d, 99 | dh_theta = theta, 100 | robot_idx = robot_idx, 101 | link_idx = link_idx, 102 | total_step = 10, 103 | obs_check_inter = 4, 104 | output_inter = 4, 105 | total_time = 1.0, 106 | fix_pose_sigma = 1e-3, 107 | fix_vel_sigma = 1e-3, 108 | cost_sigma = 0.005, 109 | hinge_loss_eps = 0.08, 110 | Qc = 1, 111 | no_report_cost = False, 112 | no_collision_exception = True) 113 | except RuntimeError as ex: 114 | print ex 115 | t = None 116 | 117 | # Run optimized trajectory 118 | try: 119 | while t is not None: 120 | raw_input('Press [Enter] to run the trajectory ...\n') 121 | with e: 122 | r.GetController().SetPath(t) 123 | except KeyboardInterrupt: 124 | print -------------------------------------------------------------------------------- /src/utils/mat.c: -------------------------------------------------------------------------------- 1 | /** \file mat.h 2 | * \brief Interface to cd_mat, a collection of useful matrix routines. 3 | */ 4 | 5 | #include 6 | #include 7 | #include 8 | #include "mat.h" 9 | 10 | int cd_mat_set_zero(double * A, int m, int n) 11 | { 12 | int i, j; 13 | for (i=0; i"); 124 | return buf; 125 | } 126 | 127 | int cd_mat_vec_fprintf(FILE * stream, const char * prefix, 128 | const double * a, int n, const char * start, const char * dblfmt, 129 | const char * sep, const char * end, const char * suffix) 130 | { 131 | int j; 132 | FILE * x_stream = stream ? stream : stdout; 133 | const char * x_prefix = prefix ? prefix : ""; 134 | /* const char * x_start = start ? start : "< "; */ 135 | const char * x_start = start ? start : " "; 136 | const char * x_dblfmt = dblfmt ? dblfmt : "%7.3f"; 137 | /* const char * x_sep = sep ? sep : ", "; */ 138 | const char * x_sep = sep ? sep : " "; 139 | /* const char * x_end = end ? end : " >"; */ 140 | const char * x_end = end ? end : " "; 141 | const char * x_suffix = suffix ? suffix : "\n"; 142 | fputs(x_prefix, x_stream); 143 | fputs(x_start, x_stream); 144 | for (j=0; j0) fputs(x_sep, x_stream); 147 | if (fabs(a[j]) < 1e-8) { 148 | fprintf(x_stream, "%7.0f", a[j]); 149 | } else { 150 | fprintf(x_stream, x_dblfmt, a[j]); 151 | } 152 | } 153 | fputs(x_end, x_stream); 154 | fputs(x_suffix, x_stream); 155 | return 0; 156 | } 157 | 158 | 159 | int cd_mat_vec_print(const char * prefix, const double * a, int n) 160 | { 161 | cd_mat_vec_fprintf(0, prefix, a, n, 0, 0, 0, 0, 0); 162 | return 0; 163 | } 164 | 165 | 166 | int cd_mat_mat_print(const char *prefix, const double *m, int row, int col) 167 | { 168 | int i; 169 | printf("%s", prefix); 170 | for (i = 0; i < row; i++) { 171 | cd_mat_vec_print(NULL, m+col*i,col); 172 | } 173 | 174 | return 0; 175 | } 176 | -------------------------------------------------------------------------------- /pythonsrc/orgpmp2/orgpmp2.py: -------------------------------------------------------------------------------- 1 | # \file orgpmp2.py 2 | # \brief Python interface to orgpmp2. 3 | 4 | import types 5 | import openravepy 6 | 7 | def bind(mod): 8 | mod.viewspheres = types.MethodType(viewspheres,mod) 9 | mod.computedistancefield = types.MethodType(computedistancefield,mod) 10 | mod.create = types.MethodType(create,mod) 11 | mod.gettraj = types.MethodType(gettraj,mod) 12 | mod.destroy = types.MethodType(destroy,mod) 13 | mod.rungpmp2 = types.MethodType(rungpmp2,mod) 14 | 15 | def shquot(s): 16 | return "'" + s.replace("'","'\\''") + "'" 17 | 18 | def viewspheres(mod, robot=None, releasegil=False): 19 | cmd = 'viewspheres' 20 | if robot is not None: 21 | if hasattr(robot,'GetName'): 22 | cmd += ' robot %s' % shquot(robot.GetName()) 23 | else: 24 | cmd += ' robot %s' % shquot(robot) 25 | print 'cmd:', cmd 26 | return mod.SendCommand(cmd, releasegil) 27 | 28 | def computedistancefield(mod, res=None, centroid=None, extents=None, 29 | cache_filename=None, save_sdf=None, releasegil=False): 30 | cmd = 'computedistancefield' 31 | if res is not None: 32 | cmd += ' res %f' % res 33 | if centroid is not None: 34 | cmd += ' centroid %s' % shquot(' '.join([str(v) for v in centroid])) 35 | if extents is not None: 36 | cmd += ' extents %s' % shquot(' '.join([str(v) for v in extents])) 37 | if cache_filename is not None: 38 | cmd += ' cache_filename %s' % shquot(cache_filename) 39 | if save_sdf is not None: 40 | cmd += ' save_sdf %d' % save_sdf 41 | print 'cmd:', cmd 42 | return mod.SendCommand(cmd, releasegil) 43 | 44 | def create(mod, robot=None, end_conf=None, base_pose=None, 45 | dh_a=None, dh_alpha=None, dh_d=None, dh_theta=None, 46 | robot_idx=None, link_idx=None, 47 | starttraj=None, total_step=None, obs_check_inter=None, output_inter=None, 48 | total_time=None, fix_pose_sigma=None, fix_vel_sigma=None, 49 | cost_sigma=None, hinge_loss_eps=None, Qc=None, 50 | save_info=None, releasegil=False, **kwargs): 51 | cmd = 'create' 52 | # robot params 53 | if robot is not None: 54 | if hasattr(robot,'GetName'): 55 | cmd += ' robot %s' % shquot(robot.GetName()) 56 | else: 57 | cmd += ' robot %s' % shquot(robot) 58 | if end_conf is not None: 59 | cmd += ' end_conf %s' % shquot(' '.join([str(v) for v in end_conf])) 60 | if base_pose is not None: 61 | cmd += ' base_pose %s' % shquot(' '.join([str(v) for v in base_pose])) 62 | if dh_a is not None: 63 | cmd += ' dh_a %s' % shquot(' '.join([str(v) for v in dh_a])) 64 | if dh_alpha is not None: 65 | cmd += ' dh_alpha %s' % shquot(' '.join([str(v) for v in dh_alpha])) 66 | if dh_d is not None: 67 | cmd += ' dh_d %s' % shquot(' '.join([str(v) for v in dh_d])) 68 | if dh_theta is not None: 69 | cmd += ' dh_theta %s' % shquot(' '.join([str(v) for v in dh_theta])) 70 | if robot_idx is not None: 71 | cmd += ' robot_idx %s' % shquot(' '.join([str(v) for v in robot_idx])) 72 | if link_idx is not None: 73 | cmd += ' link_idx %s' % shquot(' '.join([str(v) for v in link_idx])) 74 | # traj params 75 | if starttraj is not None: 76 | in_traj_data = starttraj.serialize(0) 77 | cmd += ' starttraj %s' % shquot(in_traj_data) 78 | if total_step is not None: 79 | cmd += ' total_step %d' % total_step 80 | if obs_check_inter is not None: 81 | cmd += ' obs_check_inter %d' % obs_check_inter 82 | if output_inter is not None: 83 | cmd += ' output_inter %d' % output_inter 84 | if total_time is not None: 85 | cmd += ' total_time %f' % total_time 86 | # graph params 87 | if fix_pose_sigma is not None: 88 | cmd += ' fix_pose_sigma %f' % fix_pose_sigma 89 | if fix_vel_sigma is not None: 90 | cmd += ' fix_vel_sigma %f' % fix_vel_sigma 91 | if cost_sigma is not None: 92 | cmd += ' cost_sigma %f' % cost_sigma 93 | if hinge_loss_eps is not None: 94 | cmd += ' hinge_loss_eps %f' % hinge_loss_eps 95 | if Qc is not None: 96 | cmd += ' Qc %f' % Qc 97 | # misc 98 | if save_info is not None: 99 | cmd += ' save_info %d' % save_info 100 | print 'cmd:', cmd 101 | return mod.SendCommand(cmd, releasegil) 102 | 103 | def gettraj(mod, run=None, no_collision_check=None, no_collision_exception=None, 104 | no_collision_details=None, releasegil=False): 105 | cmd = 'gettraj' 106 | if run is not None: 107 | cmd += ' run %s' % run 108 | if no_collision_check is not None and no_collision_check: 109 | cmd += ' no_collision_check' 110 | if no_collision_exception is not None and no_collision_exception: 111 | cmd += ' no_collision_exception' 112 | if no_collision_details is not None and no_collision_details: 113 | cmd += ' no_collision_details' 114 | out_traj_data = mod.SendCommand(cmd, releasegil) 115 | return openravepy.RaveCreateTrajectory(mod.GetEnv(),'').deserialize(out_traj_data) 116 | 117 | def destroy(mod, run=None, releasegil=False): 118 | cmd = 'destroy' 119 | if run is not None: 120 | cmd += ' run %s' % run 121 | return mod.SendCommand(cmd, releasegil) 122 | 123 | def rungpmp2(mod, 124 | # gettraj args 125 | no_collision_check=None, no_collision_exception=None, no_collision_details=None, 126 | releasegil=False, **kwargs): 127 | # pass unknown args to create 128 | run = create(mod, releasegil=releasegil, **kwargs) 129 | traj = gettraj(mod, run=run, 130 | no_collision_check=no_collision_check, 131 | no_collision_exception=no_collision_exception, 132 | no_collision_details=no_collision_details, 133 | releasegil=releasegil) 134 | destroy(mod, run=run, releasegil=releasegil) 135 | return traj 136 | -------------------------------------------------------------------------------- /examples/data/robots/wam7.kinbody.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 7 | 8 | 9 | -0.22 -0.14 -0.346 10 | 11 | models/WAM/wam0.iv 1.0 12 | models/WAM/wam0.iv 1.0 13 | 14 | 15 | 9.97059584 16 | 0.19982328999999999 0.14 0.079952939999999972 17 | 0.10916849 0.00640270 0.02557874 0.00640270 0.18294303 0.00161433 0.02557874 0.00161433 0.11760385 18 | 19 | 20 | 21 | 22 | wam0 23 | 0.22 0.14 0.346 24 | 1 0 0 -90 25 | 26 | 1 0 0 90 27 | models/WAM/wam1.iv 1.0 28 | models/WAM/wam1.iv 1.0 29 | 30 | 31 | 8.3936 32 | -0.00443422 0.12189039 -0.00066489 33 | 0.13488033 -0.00213041 -0.00012485 -0.00213041 0.11328369 0.00068555 -0.00012485 0.00068555 0.09046330 34 | 35 | 36 | 37 | 38 | wam0 39 | wam1 40 | wam0 41 | 0 0 1 42 | 0.22 0.14 0.346 43 | -150 150 44 | 1.5708 45 | 1.92154 46 | 0.25 47 | 0.0 0.201 0.0 48 | 77.3 49 | 50 | 51 | wam1 52 | 1 0 0 90 53 | 0 0 0 54 | 55 | 1 0 0 -90 56 | models/WAM/wam2.iv 1.0 57 | models/WAM/wam2.iv 1.0 58 | 59 | 60 | 3.87493756 61 | -0.00236983 0.03105614 0.01542114 62 | 0.02140958 0.00027172 0.00002461 0.00027172 0.01377875 -0.00181920 0.00002461 -0.00181920 0.01558906 63 | 64 | 65 | 66 | wam1 67 | wam2 68 | wam1 69 | 0 0 1 70 | -113 113 71 | 0.91739941 72 | 1.0472 73 | 0.5 74 | 0 0.182 0 75 | 160.6 76 | 77 | 78 | wam2 79 | 1 0 0 -90 80 | 0.045 0 0.55 81 | 82 | 1 0 0 90 83 | -0.045 0.55 0 84 | models/WAM/wam3.iv 1.0 85 | models/WAM/wam3.iv 1.0 86 | 87 | 88 | 1.80228141 89 | -0.03825858 0.20750770 0.00003309 90 | 0.05911077 -0.00249612 0.00000738 -0.00249612 0.00324550 -0.00001767 0.00000738 -0.00001767 0.05927043 91 | 92 | 93 | 94 | wam2 95 | wam3 96 | wam2 97 | 0 0 1 98 | -157 157 99 | 0.882397 100 | 2.0944 101 | 0.5 102 | 0 0.067 0 103 | 95.6 104 | 105 | 106 | wam3 107 | 1 0 0 90 108 | -0.045 0 0 109 | 110 | 1 0 0 -90 111 | 0.045 0 0 112 | models/WAM/wam4.iv 1.0 113 | models/WAM/wam4.iv 1.0 114 | 115 | 116 | 2.40016804 117 | 0.00498512 -0.00022942 0.13271662 118 | 0.01491672 0.00001741 -0.00150604 0.00001741 0.01482922 -0.00002109 -0.00150604 -0.00002109 0.00294463 119 | 120 | 121 | 122 | wam3 123 | wam4 124 | wam3 125 | 0 0 1 126 | -50 180 127 | 0.45504 128 | 2.0944 129 | 1 130 | 0 0.034 0 131 | 29.4 132 | 133 | 134 | wam4 135 | 0 0 0.3 136 | 1 0 0 -90 137 | 138 | 0 0.3 0 139 | 1 0 0 90 140 | models/WAM/wam5.iv 1.0 141 | models/WAM/wam5.iv 1.0 142 | 143 | 144 | 0.12376019 145 | 0.00008921 0.00511217 0.00435824 146 | 0.00005029 0.00000020 -0.00000005 0.00000020 0.00007582 -0.00000359 -0.00000005 -0.00000359 0.00006270 147 | 148 | 149 | 150 | wam4 151 | wam5 152 | wam4 153 | 0 0 1 154 | -275 75 155 | 0.40141 156 | 4.1888 157 | 1.11 158 | 0 0.0033224 0 159 | 11.6 160 | 161 | 162 | 163 | wam5 164 | 1 0 0 90 165 | 166 | 1 0 0 -90 167 | models/WAM/wam6.iv 1.0 168 | models/WAM/wam6.iv 1.0 169 | 170 | 171 | 0.41797364 172 | -0.00012262 -0.01703194 0.02468336 173 | 0.00055516 0.00000061 -0.00000074 0.00000061 0.00024367 -0.00004590 -0.00000074 -0.00004590 0.00045358 174 | 175 | 176 | 177 | 178 | wam5 179 | wam6 180 | wam5 181 | 0 0 1 182 | -90 90 183 | 0.22841245 184 | 4.1888 185 | 1.62 186 | 0 0.0033224 0 187 | 11.6 188 | 189 | 190 | wam6 191 | 0.0 0.0 0.06 192 | 193 | 0.0 0.0 -0.06 194 | models/WAM/wam7_nohand.iv 1.0 195 | models/WAM/wam7_nohand.iv 1.0 196 | 197 | 198 | 0.06864753 199 | -0.00007974 0.00016313 -0.00323552 200 | 0.00003773 -0.00000019 0.00000000 -0.00000019 0.00003806 0.00000000 0.00000000 0.00000000 0.00007408 201 | 202 | 203 | 204 | 205 | wam6 206 | wam7 207 | wam6 208 | 0 0 1 209 | -172 172 210 | 0.20178 211 | 1.0472 212 | 1.62 213 | 0 0 0.000466939 214 | 2.7 215 | 216 | 217 | wam1 wam3 218 | wam4 wam6 219 | wam4 wam7 220 | 221 | -------------------------------------------------------------------------------- /examples/data/robots/barretthand.kinbody.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 0.0 0.0 -0.06 5 | models/barrett/palm.iv 1 6 | models/barrett/palm.iv 1 7 | 8 | 9 | 0.0 0.0 0.0915 10 | -1 0 0 0 -1 0 0 0 1 11 | models/barrett/link1.iv 0.001 12 | models/barrett/link1.iv 0.001 13 | 14 | 15 | 16 | 1.1 17 | 0 0 0.04796 18 | 0.000556875 0 0 0 0.000556875 0 0 0 0.00111375 19 | 20 | 21 | 22 | 23 | 24 | handbase 25 | 0 -0.025 0.0915 26 | 27 | models/barrett/link1.iv 0.001 28 | models/barrett/link1.iv 0.001 29 | 30 | 31 | 32 | 0.1 33 | 0.026484 0.000000 -0.007214 34 | 0.000025 0 0 0 0.000069 0 0 0 0.000459 35 | 36 | 37 | 38 | Finger0-0 39 | 0.05 0 0 40 | 1 0 0 0 0 -1 0 1 0 41 | 42 | 0 0 0 43 | models/barrett/link2.iv 0.001 44 | models/barrett/link2.iv 0.001 45 | 46 | 47 | 48 | 0.1 49 | 0.033500 0.000830 0.000050 50 | 0.000009 0 0 0 0.000074 0 0 0 0.000514 51 | 52 | 53 | 54 | Finger0-0 55 | Finger0-1 56 | Finger0-1 57 | 0.03846 58 | 0 140 59 | 0 0 1 60 | 2 61 | 4.2 62 | 4.6875 63 | 64 | 65 | Finger0-1 66 | 0.07 0 0 67 | 1 0 0 0 1 0 0 0 1 68 | 69 | 0 0 0 70 | models/barrett/link3.iv 0.001 71 | models/barrett/link3.iv 0.001 72 | 73 | 74 | 75 | 0.1 76 | 0.023250 0.000000 0.000100 77 | 0.000007 0 0 0 0.000040 0 0 0 0.000352 78 | 79 | 80 | 81 | Finger0-1 82 | Finger0-2 83 | Finger0-2 84 | 0.03846 85 | 0 97 86 | 0 0 1 87 | 1 88 | 4.2 89 | 90 | 91 | 92 | 93 | handbase 94 | 0 0.025 0.0915 95 | 96 | models/barrett/link1.iv 0.001 97 | models/barrett/link1.iv 0.001 98 | 99 | 100 | 101 | 0.1 102 | 0.026484 0.000000 -0.007214 103 | 0.000025 0 0 0 0.000069 0 0 0 0.000459 104 | 105 | 106 | 107 | Finger1-0 108 | 0.05 0 0 109 | 1 0 0 0 0 -1 0 1 0 110 | 111 | 0 0 0 112 | models/barrett/link2.iv 0.001 113 | models/barrett/link2.iv 0.001 114 | 115 | 116 | 117 | 0.1 118 | 0.033500 0.000830 0.000050 119 | 0.000009 0 0 0 0.000074 0 0 0 0.000514 120 | 121 | 122 | 123 | Finger1-0 124 | Finger1-1 125 | Finger1-1 126 | 0.03846 127 | 0 140 128 | 0 0 1 129 | 2 130 | 4.2 131 | 4.6875 132 | 133 | 134 | Finger1-1 135 | 0.07 0 0 136 | 1 0 0 0 1 0 0 0 1 137 | 138 | 0 0 0 139 | models/barrett/link3.iv 0.001 140 | models/barrett/link3.iv 0.001 141 | 142 | 143 | 144 | 0.1 145 | 0.023250 0.000000 0.000100 146 | 0.000007 0 0 0 0.000040 0 0 0 0.000352 147 | 148 | 149 | 150 | Finger1-1 151 | Finger1-2 152 | Finger1-2 153 | 0.03846 154 | 0 97 155 | 0 0 1 156 | 1 157 | 4.2 158 | 159 | 160 | 161 | 162 | 163 | handbase 164 | -0.05 0 0.0915 165 | -1 0 0 0 0 1 0 1 0 166 | 167 | 0 0 0 168 | models/barrett/link2.iv 0.001 169 | models/barrett/link2.iv 0.001 170 | 171 | 172 | 173 | 0.1 174 | 0.033500 0.000830 0.000050 175 | 0.000009 0 0 0 0.000074 0 0 0 0.000514 176 | 177 | 178 | 179 | handbase 180 | Finger2-1 181 | Finger2-1 182 | 0.03846 183 | 0 140 184 | 0 0 1 185 | 2 186 | 4.2 187 | 4.6875 188 | 189 | 190 | Finger2-1 191 | 0.07 0 0 192 | 1 0 0 0 1 0 0 0 1 193 | 194 | 0 0 0 195 | models/barrett/link3.iv 0.001 196 | models/barrett/link3.iv 0.001 197 | 198 | 199 | 200 | 0.1 201 | 0.023250 0.000000 0.000100 202 | 0.000007 0 0 0 0.000040 0 0 0 0.000352 203 | 204 | 205 | 206 | Finger2-1 207 | Finger2-2 208 | Finger2-2 209 | 0.03846 210 | 0 97 211 | 0 0 1 212 | 1 213 | 4.2 214 | 215 | 216 | 217 | 218 | handbase 219 | Finger0-0 220 | Finger0-0 221 | 0.14894 222 | -1 181 223 | 0 0 -1 224 | 225 | 2 226 | 1.7 227 | 2 228 | 229 | 230 | handbase 231 | Finger1-0 232 | Finger1-0 233 | 0.14894 234 | 0 0 1 235 | 236 | 2 237 | 1.7 238 | -1 181 239 | 240 | 241 | 242 | Finger0-0 Finger1-0 243 | Finger0-1 Finger0-2 244 | Finger1-1 Finger1-2 245 | Finger2-1 Finger2-2 246 | handbase Finger0-1 247 | handbase Finger0-2 248 | handbase Finger1-1 249 | handbase Finger1-2 250 | handbase Finger2-1 251 | handbase Finger2-2 252 | 253 | -------------------------------------------------------------------------------- /examples/data/envs/countertop.env.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0.3 0.355 0.01 6 | 0.30 0.355 1.50 7 | 1 0 0 0 8 | 9 | 10 | 11 | 12 | 13 | 14 | 0.35 0.01 0.45 15 | 0.65 0.01 1.06 16 | 1 0 0 0 17 | 18 | 19 | 20 | 21 | 22 | 23 | 0.295 0.35 0.01 24 | 0.30 0.35 1.23 25 | 1 0 0 0 26 | 27 | 28 | 29 | 30 | 31 | 32 | 0.295 0.35 0.01 33 | 0.30 0.35 0.92 34 | 1 0 0 0 35 | 36 | 37 | 38 | 39 | 40 | 41 | 0.295 0.35 0.01 42 | 0.30 0.35 0.60 43 | 1 0 0 0 44 | 45 | 46 | 47 | 48 | 49 | 50 | 0.01 0.35 0.3 51 | 0.60 0.35 0.30 52 | 1 0 0 0 53 | 54 | 55 | 56 | 57 | 58 | 59 | 0.035 0.015 0.015 60 | 0.63 0.20 0.55 61 | 1 0 0 0 62 | 63 | 64 | 65 | 66 | 67 | 68 | 0.035 0.015 0.015 69 | 0.63 0.50 0.55 70 | 1 0 0 0 71 | 72 | 73 | 74 | 75 | 76 | 77 | 0.015 0.15 0.015 78 | 0.65 0.35 0.55 79 | 1 0 0 0 80 | 81 | 82 | 83 | 84 | 85 | 86 | 0.3 0.01 0.75 87 | 0.30 0.01 0.75 88 | 1 0 0 0 89 | 90 | 91 | 92 | 93 | 94 | 95 | 0.3 0.01 0.75 96 | 0.30 0.70 0.75 97 | 1 0 0 0 98 | 99 | 100 | 101 | 102 | 103 | 104 | 0.01 0.35 0.75 105 | 0.02 0.35 0.75 106 | 1 0 0 0 107 | 108 | 109 | 110 | 111 | 112 | 113 | 0.3 0.3 0.01 114 | 0.30 1.01 0.75 115 | 1 0 0 0 116 | 117 | 118 | 119 | 120 | 121 | 122 | 0.3 0.26 0.01 123 | 0.30 2.14 0.75 124 | 1 0 0 0 125 | 126 | 127 | 128 | 129 | 130 | 131 | 0.3 0.3 0.01 132 | 0.30 1.60 0.50 133 | 1 0 0 0 134 | 135 | 136 | 137 | 138 | 139 | 140 | 0.3 0.01 0.125 141 | 0.30 1.30 0.625 142 | 1 0 0 0 143 | 144 | 145 | 146 | 147 | 148 | 149 | 0.3 0.01 0.125 150 | 0.30 1.90 0.625 151 | 1 0 0 0 152 | 153 | 154 | 155 | 156 | 157 | 158 | 0.01 1.2 1.0 159 | 0.01 1.20 1.0 160 | 1 0 0 0 161 | 162 | 163 | 164 | 165 | 166 | 167 | 0.01 0.31 0.38 168 | 0.03 1.60 0.38 169 | 1 0 0 0 170 | 171 | 172 | 173 | 174 | 175 | 176 | 0.01 0.32 0.38 177 | 0.60 1.60 0.38 178 | 1 0 0 0 179 | 180 | 181 | 182 | 183 | 184 | 185 | 0.04 0.3 0.01 186 | 0.04 1.60 0.75 187 | 1 0 0 0 188 | 189 | 190 | 191 | 192 | 193 | 194 | 0.015 0.015 0.03 195 | 0.04 1.70 0.78 196 | 1 0 0 0 197 | 198 | 199 | 200 | 201 | 202 | 203 | 0.015 0.015 0.03 204 | 0.04 1.50 0.78 205 | 1 0 0 0 206 | 207 | 208 | 209 | 210 | 211 | 212 | 0.015 0.015 0.15 213 | 0.04 1.60 0.90 214 | 1 0 0 0 215 | 216 | 217 | 218 | 219 | 220 | 221 | 0.125 0.015 0.015 222 | 0.15 1.60 1.05 223 | 1 0 0 0 224 | 225 | 226 | 227 | 228 | 229 | 230 | 0.01 0.01 0.025 231 | 0.265 1.60 1.025 232 | 1 0 0 0 233 | 234 | 235 | 236 | 237 | 238 | 239 | 0.2 0.01 0.35 240 | 0.20 0.74 1.55 241 | 1 0 0 0 242 | 243 | 244 | 245 | 246 | 247 | 248 | 0.2 0.01 0.35 249 | 0.20 2.39 1.55 250 | 1 0 0 0 251 | 252 | 253 | 254 | 255 | 256 | 257 | 0.2 0.01 0.35 258 | 0.20 1.62 1.55 259 | 1 0 0 0 260 | 261 | 262 | 263 | 264 | 265 | 266 | 0.2 0.835 0.01 267 | 0.20 1.565 1.20 268 | 1 0 0 0 269 | 270 | 271 | 272 | 273 | 274 | 275 | 0.2 0.835 0.01 276 | 0.20 1.565 1.90 277 | 1 0 0 0 278 | 279 | 280 | 281 | 282 | 283 | 284 | 0.2 0.835 0.01 285 | 0.20 1.565 1.55 286 | 1 0 0 0 287 | 288 | 289 | 290 | 291 | 292 | 293 | 0.3 0.01 0.38 294 | 0.30 1.29 0.38 295 | 1 0 0 0 296 | 297 | 298 | 299 | 300 | 301 | 302 | 0.3 0.01 0.38 303 | 0.30 0.72 0.38 304 | 1 0 0 0 305 | 306 | 307 | 308 | 309 | 310 | 311 | 0.01 0.3 0.375 312 | 0.02 1.00 0.37 313 | 1 0 0 0 314 | 315 | 316 | 317 | 318 | 319 | 320 | 0.3 0.3 0.01 321 | 0.30 1.00 0.35 322 | 1 0 0 0 323 | 324 | 325 | 326 | 327 | 328 | 329 | 0.3 0.3 0.01 330 | 0.30 1.00 0.01 331 | 1 0 0 0 332 | 333 | 334 | 335 | 336 | 337 | 338 | 0.02 0.02 0.12 339 | 0.43 1.92 0.87 340 | 1 0 0 0 341 | 342 | 343 | 344 | 345 | 346 | 347 | 0.025 0.025 0.08 348 | 0.45 1.97 0.81 349 | 1 0 0 0 350 | 351 | 352 | 353 | 354 | 355 | 356 | 0.03 0.03 0.16 357 | 0.36 1.94 0.92 358 | 1 0 0 0 359 | 360 | 361 | 362 | 363 | 364 | 365 | 0.3 0.01 0.375 366 | 0.30 2.39 0.375 367 | 1 0 0 0 368 | 369 | 370 | 371 | 372 | 373 | 374 | 0.3 0.01 0.375 375 | 0.30 1.91 0.375 376 | 1 0 0 0 377 | 378 | 379 | 380 | 381 | 382 | 383 | 0.01 0.24 0.08 384 | 0.60 2.16 0.08 385 | 1 0 0 0 386 | 387 | 388 | 389 | 390 | 391 | 392 | 0.01 0.24 0.04 393 | 0.61 2.16 0.72 394 | 1 0 0 0 395 | 396 | 397 | 398 | 399 | 400 | 401 | 0.01 0.24 0.37 402 | 0.02 2.15 0.35 403 | 1 0 0 0 404 | 405 | 406 | 407 | 408 | 409 | 410 | 0.3 0.24 0.01 411 | 0.90 2.16 0.16 412 | 1 0 0 0 413 | 414 | 415 | 416 | 417 | 418 | 419 | 0.28 0.24 0.01 420 | 0.30 2.15 0.10 421 | 1 0 0 0 422 | 423 | 424 | 425 | 426 | 427 | 428 | 0.015 0.015 0.035 429 | 1.10 2.00 0.12 430 | 1 0 0 0 431 | 432 | 433 | 434 | 435 | 436 | 437 | 0.015 0.015 0.035 438 | 1.10 2.3 0.12 439 | 1 0 0 0 440 | 441 | 442 | 443 | 444 | 445 | 446 | 0.015 0.165 0.015 447 | 1.10 2.15 0.08 448 | 1 0 0 0 449 | 450 | 451 | 452 | 453 | 454 | 455 | 0.1 0.015 0.13 456 | 0.38 2.34 0.89 457 | 1 0 0 0 458 | 459 | 460 | 461 | 462 | 463 | 464 | 0.1 0.01 0.15 465 | 0.41 2.37 0.92 466 | 1 0 0 0 467 | 468 | 469 | 470 | -------------------------------------------------------------------------------- /examples/data/problemsets.py: -------------------------------------------------------------------------------- 1 | import sys 2 | 3 | def active_affine(problemset): 4 | if problemset == 'countertop': 5 | return 0 6 | if problemset == 'bookshelves': 7 | return 0 8 | if problemset == 'industrial': 9 | return 0 10 | if problemset == 'industrial2': 11 | return 0 12 | if problemset == 'tunnel': 13 | return 0 14 | else: 15 | print "Unknown problem set" 16 | sys.exit() 17 | 18 | def active_joints(problemset): 19 | if problemset == 'countertop': 20 | return ['r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 21 | 'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'] 22 | if problemset == 'bookshelves': 23 | return ['r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 24 | 'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'] 25 | if problemset == 'industrial': 26 | return ['r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 27 | 'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'] 28 | if problemset == 'industrial2': 29 | return ['r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 30 | 'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'] 31 | if problemset == 'tunnel': 32 | return ['r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 33 | 'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'] 34 | else: 35 | print "Unknown problem set" 36 | sys.exit() 37 | 38 | def default_base_pose(problemset): 39 | if problemset == 'countertop': 40 | return [0.03074371707199644, -0.0, -0.0, -0.9995273002077517, 1.0746809244155884, 41 | 1.500715732574463, 0.0] 42 | if problemset == 'bookshelves': 43 | return [1.0, 0.0, 0.0, 0.0, 0.11382412910461426, 0.0, 0.0] 44 | if problemset == 'industrial': 45 | return [0.9999554696772218, 0.0, 0.0, 0.009437089731840358, 0.10682493448257446, 46 | -0.09225612878799438, 0.0] 47 | if problemset == 'industrial2': 48 | return [0.9074254167930225, 0.0, 0.0, -0.42021317561210453, -0.0048143863677978516, 49 | 0.039366304874420166, 0.0] 50 | if problemset == 'tunnel': 51 | return [0.9999523740218402, 0.0, 0.0, 0.00975959466810749, -0.013043247163295746, 52 | -0.2435353398323059, 0.0] 53 | else: 54 | print "Unknown problem set" 55 | sys.exit() 56 | 57 | def default_joint_values(problemset): 58 | if problemset == 'countertop': 59 | return [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 60 | 0.16825, 0.0, 0.0, 0.0, -0.8451866735633127, 0.08128204585620136, -1.8311522093787793, 61 | -1.5550420276338315, 2.6911049983477024, -0.8325789594278508, 2.067980015738538, 62 | 2.248201624865942e-15, 0.0, 0.0, 0.0, 0.0, 0.0, -1.1102230246251565e-16, 1.5301977050596074, 63 | -0.1606585554274158, 1.122, -2.1212501013292435, 2.5303972717977263, -0.7028113314291433, 64 | 1.925250846169634, -7.494005416219807e-15, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 65 | if problemset == 'bookshelves': 66 | return [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 67 | 0.0, 0.16825, 0.0, 0.0, 0.0, -0.5652894131595758, -0.1940789551546196, -1.260201738335192, 68 | -0.7895653603354864, -2.322747882942366, -0.3918504494615993, -2.5173485998351066, 69 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.1347678577910827, 0.05595277251194286, 0.48032314980402596, 70 | -2.0802263633096487, 1.2294916701952125, -0.8773017824611689, 2.932954218704465, 71 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 72 | if problemset == 'industrial': 73 | return [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 74 | 0.16825, 0.0, 0.0, 0.0, 0.4521781848795534, -0.06875607890205337, 0.45157478971034287, 75 | -1.356432823197765, 2.63664188822003, -1.2884530258626397, 2.7514905004419816, 2.248201624865942e-15, 76 | 0.0, 0.0, 0.0, 0.0, 0.0, -1.1102230246251565e-16, 1.1473791555597268, -0.2578419004077155, 77 | 0.5298918609954418, -2.121201719392923, 2.198118788614387, -1.4189668927954484, 78 | 2.1828521334438378, -4.08006961549745e-15, 0.0, 0.0, 0.0, 0.0, 0.0, 7.549516567451064e-15, 79 | 0.0] 80 | if problemset == 'industrial2': 81 | return [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 82 | 0.16825, 0.0, 0.0, 0.0, -0.2761549481623158, 0.8939542514393661, -0.4462472669817504, 83 | -1.6105027440487039, 2.328001213611822, -1.0881333252440992, -2.3998853474978716, 84 | 2.248201624865942e-15, 0.0, 0.0, 0.0, 0.0, 0.0, -1.1102230246251565e-16, 1.1473791555597268, 85 | -0.2578419004077155, 0.5298918609954418, -2.121201719392923, 2.198118788614387, 86 | -1.4189668927954484, 2.1828521334438378, -1.174060848541103e-14, 0.0, 0.0, 0.0, 87 | 0.0, 0.0, -1.6653345369377348e-16, 0.0] 88 | if problemset == 'tunnel': 89 | return [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 90 | 0.16825, 0.0, 0.0, 0.0, -0.37279882212870064, 1.0259015008778194, -0.9875997438281771, 91 | -1.208229229103619, 2.0676739431952065, -0.5630237954661839, -1.6563473012595384, 92 | 2.248201624865942e-15, 0.0, 0.0, 0.0, 0.0, 0.0, -1.1102230246251565e-16, 1.1473791555597268, 93 | -0.2578419004077155, 0.5298918609954418, -2.121201719392923, 2.198118788614387, 94 | -1.4189668927954484, 2.1828521334438378, -1.2961853812498703e-14, 0.0, 0.0, 0.0, 95 | 0.0, 0.0, 0.0, 0.0] 96 | else: 97 | print "Unknown problem set" 98 | sys.exit() 99 | 100 | def env_file(problemset): 101 | if problemset == 'countertop': 102 | return "data/envs/countertop.env.xml" 103 | if problemset == 'bookshelves': 104 | return "data/envs/bookshelves.env.xml" 105 | if problemset == 'industrial': 106 | return "data/envs/industrial.env.xml" 107 | if problemset == 'industrial2': 108 | return "data/envs/industrial.env.xml" 109 | if problemset == 'tunnel': 110 | return "data/envs/tunnel.env.xml" 111 | if problemset == 'lab': 112 | return "data/envs/lab.env.xml" 113 | else: 114 | print "Unknown problem set" 115 | sys.exit() 116 | 117 | def joint_names(problemset): 118 | if problemset == 'countertop': 119 | return ['fl_caster_rotation_joint', 'fl_caster_l_wheel_joint', 'fl_caster_r_wheel_joint', 120 | 'fr_caster_rotation_joint', 'fr_caster_l_wheel_joint', 'fr_caster_r_wheel_joint', 'bl_caster_rotation_joint', 121 | 'bl_caster_l_wheel_joint', 'bl_caster_r_wheel_joint', 'br_caster_rotation_joint', 'br_caster_l_wheel_joint', 122 | 'br_caster_r_wheel_joint', 'torso_lift_joint', 'head_pan_joint', 'head_tilt_joint', 'laser_tilt_mount_joint', 123 | 'r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 124 | 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint', 'r_gripper_motor_slider_joint', 125 | 'r_gripper_motor_screw_joint', 'r_gripper_l_finger_joint', 'r_gripper_l_finger_tip_joint', 126 | 'r_gripper_r_finger_joint', 'r_gripper_r_finger_tip_joint', 'r_gripper_joint', 'l_shoulder_pan_joint', 127 | 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint', 'l_elbow_flex_joint', 'l_forearm_roll_joint', 128 | 'l_wrist_flex_joint', 'l_wrist_roll_joint', 'l_gripper_motor_slider_joint', 'l_gripper_motor_screw_joint', 129 | 'l_gripper_l_finger_joint', 'l_gripper_l_finger_tip_joint', 'l_gripper_r_finger_joint', 130 | 'l_gripper_r_finger_tip_joint', 'l_gripper_joint', 'torso_lift_motor_screw_joint'] 131 | if problemset == 'bookshelves': 132 | return ['torso_lift_motor_screw_joint', 'fl_caster_rotation_joint', 'fl_caster_l_wheel_joint', 'fl_caster_r_wheel_joint', 133 | 'fr_caster_rotation_joint', 'fr_caster_l_wheel_joint', 'fr_caster_r_wheel_joint', 'bl_caster_rotation_joint', 134 | 'bl_caster_l_wheel_joint', 'bl_caster_r_wheel_joint', 'br_caster_rotation_joint', 'br_caster_l_wheel_joint', 135 | 'br_caster_r_wheel_joint', 'torso_lift_joint', 'head_pan_joint', 'head_tilt_joint', 'laser_tilt_mount_joint', 136 | 'r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 137 | 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint', 'r_gripper_motor_slider_joint', 138 | 'r_gripper_motor_screw_joint', 'r_gripper_l_finger_joint', 'r_gripper_l_finger_tip_joint', 139 | 'r_gripper_r_finger_joint', 'r_gripper_r_finger_tip_joint', 'r_gripper_joint', 'l_shoulder_pan_joint', 140 | 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint', 'l_elbow_flex_joint', 'l_forearm_roll_joint', 141 | 'l_wrist_flex_joint', 'l_wrist_roll_joint', 'l_gripper_motor_slider_joint', 'l_gripper_motor_screw_joint', 142 | 'l_gripper_l_finger_joint', 'l_gripper_l_finger_tip_joint', 'l_gripper_r_finger_joint', 143 | 'l_gripper_r_finger_tip_joint', 'l_gripper_joint'] 144 | if problemset == 'industrial': 145 | return ['fl_caster_rotation_joint', 'fl_caster_l_wheel_joint', 'fl_caster_r_wheel_joint', 146 | 'fr_caster_rotation_joint', 'fr_caster_l_wheel_joint', 'fr_caster_r_wheel_joint', 'bl_caster_rotation_joint', 147 | 'bl_caster_l_wheel_joint', 'bl_caster_r_wheel_joint', 'br_caster_rotation_joint', 'br_caster_l_wheel_joint', 148 | 'br_caster_r_wheel_joint', 'torso_lift_joint', 'head_pan_joint', 'head_tilt_joint', 'laser_tilt_mount_joint', 149 | 'r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 150 | 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint', 'r_gripper_motor_slider_joint', 151 | 'r_gripper_motor_screw_joint', 'r_gripper_l_finger_joint', 'r_gripper_l_finger_tip_joint', 152 | 'r_gripper_r_finger_joint', 'r_gripper_r_finger_tip_joint', 'r_gripper_joint', 'l_shoulder_pan_joint', 153 | 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint', 'l_elbow_flex_joint', 'l_forearm_roll_joint', 154 | 'l_wrist_flex_joint', 'l_wrist_roll_joint', 'l_gripper_motor_slider_joint', 'l_gripper_motor_screw_joint', 155 | 'l_gripper_l_finger_joint', 'l_gripper_l_finger_tip_joint', 'l_gripper_r_finger_joint', 156 | 'l_gripper_r_finger_tip_joint', 'l_gripper_joint', 'torso_lift_motor_screw_joint'] 157 | if problemset == 'industrial2': 158 | return ['fl_caster_rotation_joint', 'fl_caster_l_wheel_joint', 'fl_caster_r_wheel_joint', 159 | 'fr_caster_rotation_joint', 'fr_caster_l_wheel_joint', 'fr_caster_r_wheel_joint', 'bl_caster_rotation_joint', 160 | 'bl_caster_l_wheel_joint', 'bl_caster_r_wheel_joint', 'br_caster_rotation_joint', 'br_caster_l_wheel_joint', 161 | 'br_caster_r_wheel_joint', 'torso_lift_joint', 'head_pan_joint', 'head_tilt_joint', 'laser_tilt_mount_joint', 162 | 'r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 163 | 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint', 'r_gripper_motor_slider_joint', 164 | 'r_gripper_motor_screw_joint', 'r_gripper_l_finger_joint', 'r_gripper_l_finger_tip_joint', 165 | 'r_gripper_r_finger_joint', 'r_gripper_r_finger_tip_joint', 'r_gripper_joint', 'l_shoulder_pan_joint', 166 | 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint', 'l_elbow_flex_joint', 'l_forearm_roll_joint', 167 | 'l_wrist_flex_joint', 'l_wrist_roll_joint', 'l_gripper_motor_slider_joint', 'l_gripper_motor_screw_joint', 168 | 'l_gripper_l_finger_joint', 'l_gripper_l_finger_tip_joint', 'l_gripper_r_finger_joint', 169 | 'l_gripper_r_finger_tip_joint', 'l_gripper_joint', 'torso_lift_motor_screw_joint'] 170 | if problemset == 'tunnel': 171 | return ['fl_caster_rotation_joint', 'fl_caster_l_wheel_joint', 'fl_caster_r_wheel_joint', 172 | 'fr_caster_rotation_joint', 'fr_caster_l_wheel_joint', 'fr_caster_r_wheel_joint', 'bl_caster_rotation_joint', 173 | 'bl_caster_l_wheel_joint', 'bl_caster_r_wheel_joint', 'br_caster_rotation_joint', 'br_caster_l_wheel_joint', 174 | 'br_caster_r_wheel_joint', 'torso_lift_joint', 'head_pan_joint', 'head_tilt_joint', 'laser_tilt_mount_joint', 175 | 'r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 176 | 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint', 'r_gripper_motor_slider_joint', 177 | 'r_gripper_motor_screw_joint', 'r_gripper_l_finger_joint', 'r_gripper_l_finger_tip_joint', 178 | 'r_gripper_r_finger_joint', 'r_gripper_r_finger_tip_joint', 'r_gripper_joint', 'l_shoulder_pan_joint', 179 | 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint', 'l_elbow_flex_joint', 'l_forearm_roll_joint', 180 | 'l_wrist_flex_joint', 'l_wrist_roll_joint', 'l_gripper_motor_slider_joint', 'l_gripper_motor_screw_joint', 181 | 'l_gripper_l_finger_joint', 'l_gripper_l_finger_tip_joint', 'l_gripper_r_finger_joint', 182 | 'l_gripper_r_finger_tip_joint', 'l_gripper_joint', 'torso_lift_motor_screw_joint'] 183 | else: 184 | print "Unknown problem set" 185 | sys.exit() 186 | 187 | def states(problemset): 188 | if problemset == 'countertop': 189 | n_states = 9 190 | states = [0 for i in range(n_states)] 191 | states[0] = [-0.8452, 0.0813, -1.8312, -1.555 , 2.6911, -0.8326, 2.068 ] 192 | states[1] = [-0.4134, -0.238 , -3.6504, -1.1768, 2.7225, -1.2706, -2.3709] 193 | states[2] = [-0.9547, 0.3356, -2.3151, -0.9126, 1.8166, -0.8724, -3.1287] 194 | states[3] = [-0.496 , -0.2946, -2.626 , -1.5671, -0.9644, -0.5307, 0.5828] 195 | states[4] = [-0.978 , -0.235 , -1.3629, -1.282 , -2.2903, -0.4913, 0.9081] 196 | states[5] = [-0.3043, -0.1995, 0.4997, -0.9161, -3.0128, -1.2772, -0.4844] 197 | states[6] = [-0.0826, -0.3115, -0.7685, -1.0468, -2.8332, -1.2915, 0.7087] 198 | states[7] = [-0.9493, -0.2259, -1.2924, -1.2902, -2.2911, -0.5655, -2.1449] 199 | states[8] = [-0.0077, -0.1813, -1.2825, -0.2072, -2.475 , -0.3674, -2.5659] 200 | return n_states, states 201 | if problemset == 'bookshelves': 202 | n_states = 10 203 | states = [0 for i in range(n_states)] 204 | states[0] = [-0.5653, -0.1941, -1.2602, -0.7896, -2.3227, -0.3919, -2.5173] 205 | states[1] = [-0.1361, -0.1915, -1.2602, -0.8652, -2.8852, -0.7962, -2.039 ] 206 | states[2] = [ 0.2341, -0.2138, -1.2602, -0.4709, -3.0149, -0.7505, -2.0164] 207 | states[3] = [ 0.1584, 0.3429, -1.2382, -0.9829, -2.0892, -1.6126, -0.5582] 208 | states[4] = [ 0.3927, 0.1763, -1.2382, -0.1849, -1.96 , -1.4092, -1.0492] 209 | states[5] = [-0.632 , 0.5012, -1.2382, -0.8353, 2.2571, -0.1041, 0.3066] 210 | states[6] = [ 0.1683, 0.7154, -0.4195, -1.0496, 2.4832, -0.6028, -0.6401] 211 | states[7] = [-0.1198, 0.5299, -0.6291, -0.4348, 2.1715, -1.6403, 1.8299] 212 | states[8] = [ 0.2743, 0.4088, -0.5291, -0.4304, 2.119 , -1.9994, 1.7162] 213 | states[9] = [ 0.2743, 0.4088, -0.5291, -0.4304, -0.9985, -1.0032, -1.7278] 214 | return n_states, states 215 | if problemset == 'industrial': 216 | n_states = 12 217 | states = [0 for i in range(n_states)] 218 | states[0] = [ 0.4522, -0.0688, 0.4516, -1.3564, 2.6366, -1.2885, 2.7515] 219 | states[1] = [ 0.5645, -0.2949, -0.8384, -0.4594, 2.9742, -1.1313, -2.3144] 220 | states[2] = [ 0.2846, -0.3534, -2.2822, -1.2794, -2.6088, -1.298 , -1.1206] 221 | states[3] = [ 0.281 , 0.7666, 0.0846, -1.8104, 2.8603, -1.1027, -3.1227] 222 | states[4] = [ 0.0514, -0.3534, -2.8542, -1.7384, -2.95 , -1.3661, -0.3652] 223 | states[5] = [ 0.2874, -0.0535, -2.3339, -1.2294, -2.8416, -1.3918, -0.9476] 224 | states[6] = [ 0.5259, 0.9754, -0.2409, -0.9607, 1.7601, -0.748 , -1.6548] 225 | states[7] = [-0.4355, 1.2821, -0.5184, -1.4671, 2.7845, -0.2334, -2.709 ] 226 | states[8] = [-1.0536, -0.3436, -1.6586, -1.4779, -2.7631, -0.4247, -1.6377] 227 | states[9] = [-0.7221, -0.3534, -1.639 , -1.7309, -2.8092, -0.9864, -1.5293] 228 | states[10] = [-0.3357, -0.1715, -0.3289, -2.0001, 1.6523, -1.9265, 2.5474] 229 | states[11] = [-0.3888, 1.0477, -1.1405, -0.7096, 0.9253, -0.5049, -0.3575] 230 | return n_states, states 231 | if problemset == 'industrial2': 232 | n_states = 10 233 | states = [0 for i in range(n_states)] 234 | states[0] = [-0.2762, 0.894 , -0.4462, -1.6105, 2.328 , -1.0881, -2.3999] 235 | states[1] = [-0.571 , -0.3534, -1.7684, -1.5384, -2.7693, -1.5537, -1.4818] 236 | states[2] = [-0.1708, 0.093 , -1.1193, -1.0388, 2.8681, -1.4571, -1.9924] 237 | states[3] = [ 0.2855, -0.198 , -3.2144, -1.0791, -2.0565, -1.1219, -0.6414] 238 | states[4] = [-0.5471, 0.1817, -1.835 , -1.6187, 3.0026, -1.7675, -1.3979] 239 | states[5] = [ 0.1713, 0.377 , -0.5826, -0.5771, -1.5264, -0.4143, 2.0764] 240 | states[6] = [ 0.514 , 0.2662, -1.2524, -0.6177, 2.9156, -0.2591, -1.7356] 241 | states[7] = [ 0.5512, -0.3535, -1.2124, -0.4724, -2.1021, -0.5965, -2.8023] 242 | states[8] = [ 0.5272, 0.7193, -0.9876, -0.5453, 1.2938, -0.3151, -0.5195] 243 | states[9] = [-0.3728, 1.0259, -0.9876, -1.2082, 2.0042, -1.3781, -1.6173] 244 | return n_states, states 245 | if problemset == 'tunnel': 246 | n_states = 4 247 | states = [0 for i in range(n_states)] 248 | states[0] = [-0.3728, 1.0259, -0.9876, -1.2082, 2.0677, -0.563 , -1.6563] 249 | states[1] = [ 0.2993, 0.1747, -1.7835, -0.8593, 3.1042, -1.2145, -1.3626] 250 | states[2] = [ 0.3537, 0.2079, -1.7168, -0.2937, 3.0224, -0.7083, -1.3286] 251 | states[3] = [ 0.3398, -0.2349, -0.0415, -1.5042, 2.7647, -1.7995, 3.0539] 252 | return n_states, states 253 | if problemset == 'lab': 254 | starts = 4 255 | ends = 6 256 | start = [[0 for i in range(7)] for x in range(starts)] 257 | end = [[0 for i in range(7)] for x in range(ends)] 258 | start[0] = [ 1.580, 1.100, 0.000, 1.700, 0.000, -1.240, 1.570] 259 | start[1] = [ -0.605, -1.710, 1.640, 1.290, 1.200, -0.106, 2.000] 260 | start[2] = [ 1.650, 1.400, 0.000, 0.900, 0.000, -0.800, 1.570] 261 | start[3] = [ 2.100, 1.000, 0.000, 1.700, 0.000, -1.000, 1.570] 262 | end[0] = [ 0.000, 0.600, 0.000, 1.300, 0.000, -0.250, 1.570] 263 | end[1] = [ 0.260, 0.600, 0.000, 1.300, 0.000, -0.250, 1.570] 264 | end[2] = [ -0.260, 0.600, 0.000, 1.300, 0.000, -0.250, 1.570] 265 | end[3] = [ 0.000, 0.970, 0.000, 1.600, 0.000, -0.919, 1.570] 266 | end[4] = [ 0.260, 0.970, 0.000, 1.600, 0.000, -0.919, 1.570] 267 | end[5] = [ -0.260, 0.970, 0.000, 1.600, 0.000, -0.919, 1.570] 268 | return starts, ends, start, end 269 | else: 270 | print "Unknown problem set" 271 | sys.exit() 272 | -------------------------------------------------------------------------------- /src/utils/grid.c: -------------------------------------------------------------------------------- 1 | /** 2 | * \file grid.c 3 | * \brief Implementation of cd_grid, a multidimensional grid of values. 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include "grid.h" 11 | 12 | /* Rectgrid, standard c-ordered indexing into nd grid: 13 | * grid(x,y,z) = grid[x][y][z] = grid[x*NY*NZ + y*NZ + z] */ 14 | int cd_grid_create(struct cd_grid ** gp, 15 | void * cell_init, int cell_size, int n, ...) 16 | { 17 | size_t i; 18 | va_list argp; 19 | int * sizes; 20 | 21 | /* Fill sizes from n variadic arguments */ 22 | sizes = (int *) malloc(n * sizeof(int)); 23 | if (!sizes) { return -1; } 24 | va_start(argp, n); 25 | for (i=0; in = n; 51 | g->sizes = sizes; 52 | g->ncells = 0; 53 | g->cell_size = cell_size; 54 | g->data = 0; 55 | g->lengths = 0; 56 | 57 | /* Allocate cell data */ 58 | g->ncells = 1; 59 | for (i=0; incells *= g->sizes[i]; 61 | if (cell_size > 0) 62 | { 63 | g->data = (char *) malloc(g->ncells * cell_size); 64 | if (!g->data) { cd_grid_destroy(g); return -2; } 65 | 66 | /* Initialize cell data */ 67 | for (i=0; incells; i++) 68 | memcpy(g->data + i*cell_size, cell_init, cell_size); 69 | } 70 | 71 | /* Allocate lengths vector */ 72 | g->lengths = (double *) malloc(n*sizeof(double)); 73 | if (!g->lengths) { cd_grid_destroy(g); return -1; } 74 | for (i=0; ilengths[i] = 1.0; 75 | 76 | *gp = g; 77 | return 0; 78 | } 79 | 80 | int cd_grid_create_copy(struct cd_grid ** gp, 81 | struct cd_grid * gsrc) 82 | { 83 | struct cd_grid * g; 84 | 85 | g = (struct cd_grid *) malloc(sizeof(struct cd_grid)); 86 | if (!g) return -1; 87 | g->n = gsrc->n; 88 | g->sizes = 0; 89 | g->ncells = gsrc->ncells; 90 | g->cell_size = gsrc->cell_size; 91 | g->data = 0; 92 | 93 | /* Fill sizes from n variadic arguments */ 94 | g->sizes = (int *) malloc(gsrc->n * sizeof(int)); 95 | if (!g->sizes) { cd_grid_destroy(g); return -1; } 96 | memcpy(g->sizes, gsrc->sizes, gsrc->n * sizeof(int)); 97 | 98 | /* Allocate cell data */ 99 | if (g->cell_size > 0) 100 | { 101 | g->data = (char *) malloc(g->ncells * g->cell_size); 102 | if (!g->data) { cd_grid_destroy(g); return -2; } 103 | memcpy(g->data, gsrc->data, g->ncells * g->cell_size); 104 | } 105 | 106 | /* Allocate lengths vector */ 107 | g->lengths = (double *) malloc(gsrc->n*sizeof(double)); 108 | if (!g->lengths) { cd_grid_destroy(g); return -1; } 109 | memcpy(g->lengths, gsrc->lengths, gsrc->n*sizeof(double)); 110 | 111 | *gp = g; 112 | return 0; 113 | } 114 | 115 | int cd_grid_destroy(struct cd_grid * g) 116 | { 117 | if (!g) 118 | return 0; 119 | free(g->data); 120 | free(g->sizes); 121 | free(g->lengths); 122 | free(g); 123 | return 0; 124 | } 125 | 126 | int cd_grid_index_to_subs(struct cd_grid * g, size_t index, int * subs) 127 | { 128 | int ni; 129 | size_t index_remain; 130 | 131 | index_remain = index; 132 | for (ni=g->n-1; ni>=0; ni--) 133 | { 134 | subs[ni] = index_remain % g->sizes[ni]; 135 | index_remain = index_remain / g->sizes[ni]; 136 | } 137 | 138 | return 0; 139 | } 140 | 141 | int cd_grid_index_from_subs(struct cd_grid * g, size_t * index, int * subs) 142 | { 143 | int ni; 144 | *index = subs[0]; 145 | for (ni=1; nin; ni++) 146 | { 147 | *index *= g->sizes[ni]; 148 | *index += subs[ni]; 149 | } 150 | return 0; 151 | } 152 | 153 | int cd_grid_center_index(struct cd_grid * g, 154 | size_t index, double * center) 155 | { 156 | int ni; 157 | int sub; 158 | size_t index_remain; 159 | 160 | index_remain = index; 161 | for (ni=g->n-1; ni>=0; ni--) 162 | { 163 | sub = index_remain % g->sizes[ni]; 164 | index_remain = index_remain / g->sizes[ni]; 165 | center[ni] = (0.5 + sub) / g->sizes[ni]; 166 | } 167 | 168 | for (ni=0; nin; ni++) center[ni] *= g->lengths[ni]; 169 | return 0; 170 | } 171 | 172 | int cd_grid_lookup_index(struct cd_grid * g, 173 | double * p, size_t * index) 174 | { 175 | int ni; 176 | double x; 177 | int sub; 178 | *index = 0; 179 | for (ni=0; nin; ni++) 180 | { 181 | x = p[ni] / g->lengths[ni]; 182 | if (x < 0.0) return 1; 183 | if (x > 1.0) return 1; 184 | sub = (int) floor(x * g->sizes[ni]); 185 | if (sub == g->sizes[ni]) sub--; 186 | *index *= g->sizes[ni]; 187 | *index += sub; 188 | } 189 | return 0; 190 | } 191 | 192 | int cd_grid_lookup_subs(struct cd_grid * g, 193 | double * p, int * subs) 194 | { 195 | int ni; 196 | int sub; 197 | double x; 198 | for (ni=0; nin; ni++) 199 | { 200 | x = p[ni] / g->lengths[ni]; 201 | if (x < 0.0) return 1; 202 | if (x > 1.0) return 1; 203 | sub = (int) floor(x * g->sizes[ni]); 204 | if (sub == g->sizes[ni]) sub--; 205 | subs[ni] = sub; 206 | } 207 | return 0; 208 | } 209 | 210 | void * cd_grid_get_index(struct cd_grid * g, size_t index) 211 | { 212 | return g->data + index*g->cell_size; 213 | } 214 | 215 | void * cd_grid_get_subs(struct cd_grid * g, int * subs) 216 | { 217 | int ni; 218 | size_t index; 219 | index = subs[0]; 220 | for (ni=1; nin; ni++) 221 | { 222 | index *= g->sizes[ni]; 223 | index += subs[ni]; 224 | } 225 | return g->data + index*g->cell_size; 226 | } 227 | 228 | void * cd_grid_get(struct cd_grid * g, ...) 229 | { 230 | int ni; 231 | va_list argp; 232 | size_t index; 233 | va_start(argp, g); 234 | index = va_arg(argp, int); 235 | for (ni=1; nin; ni++) 236 | { 237 | index *= g->sizes[ni]; 238 | index += va_arg(argp, int); 239 | } 240 | va_end(argp); 241 | return g->data + index*g->cell_size; 242 | } 243 | 244 | 245 | /* func: intput, length n 246 | * trans: output, length n 247 | * v: temp, length n 248 | * z: temp, length n+1 249 | */ 250 | static int sedt_onedim(int n, double * func, double * trans, int trans_stride, 251 | int * v, double * z) 252 | { 253 | int i; 254 | int np; /* np = num parabolas = k+1 */ 255 | int q; 256 | double s; 257 | 258 | np = 0; 259 | 260 | /* Consider each cell, compute lower envelope */ 261 | for (q=0; qn-1; ni>=0; ni--) 334 | { 335 | /* Get subscript, decumulate index_remain */ 336 | sub = index_remain % g->sizes[ni]; 337 | index_remain = index_remain / g->sizes[ni]; 338 | /* Decide whether to use previous or next cell */ 339 | if (sub == 0) goto use_next; 340 | if (sub == g->sizes[ni]-1) goto use_previous; 341 | /* Get center location */ 342 | center = (0.5 + sub) / g->sizes[ni] * g->lengths[ni]; 343 | if (p[ni] < center) 344 | goto use_previous; 345 | else 346 | goto use_next; 347 | /* Calculate the difference */ 348 | use_previous: 349 | /* Use previous cell for gradient */ 350 | diff = *(double *)cd_grid_get_index(g,index); 351 | diff -= *(double *)cd_grid_get_index(g,index - stride); 352 | goto save_continue; 353 | use_next: 354 | /* Use next cell for gradient */ 355 | diff = *(double *)cd_grid_get_index(g,index + stride); 356 | diff -= *(double *)cd_grid_get_index(g,index); 357 | goto save_continue; 358 | /* Save to the grad vector, accumulate the stride */ 359 | save_continue: 360 | grad[ni] = diff * g->sizes[ni] / g->lengths[ni]; 361 | stride *= g->sizes[ni]; 362 | } 363 | 364 | return 0; 365 | } 366 | 367 | int cd_grid_double_interp(struct cd_grid * g, double * p, double * valuep) 368 | { 369 | int err; 370 | int ni; 371 | size_t stride; 372 | size_t index; 373 | size_t index_remain; 374 | int sub; 375 | double center; 376 | double diff; 377 | double value; 378 | double grad; 379 | double value_after; 380 | double value_before; 381 | 382 | /* First, look up the value at the nearest center */ 383 | err = cd_grid_lookup_index(g, p, &index); 384 | if (err) return err; 385 | value = *(double *)cd_grid_get_index(g,index); 386 | if (value == HUGE_VAL) 387 | { *valuep = HUGE_VAL; return 0; } 388 | 389 | /* This is identical to the gradient algorithm from above */ 390 | stride = 1; 391 | index_remain = index; 392 | for (ni=g->n-1; ni>=0; ni--) 393 | { 394 | /* Get subscript, decumulate index_remain */ 395 | sub = index_remain % g->sizes[ni]; 396 | index_remain = index_remain / g->sizes[ni]; 397 | /* Get center location */ 398 | center = (0.5 + sub) / g->sizes[ni] * g->lengths[ni]; 399 | /* Decide whether to use previous or next cell */ 400 | if (sub == 0) goto use_next; 401 | if (sub == g->sizes[ni]-1) goto use_previous; 402 | if (p[ni] < center) 403 | goto use_previous; 404 | else 405 | goto use_next; 406 | /* Calculate the difference */ 407 | use_previous: 408 | /* Use previous cell for gradient */ 409 | value_after = *(double *)cd_grid_get_index(g,index); 410 | value_before = *(double *)cd_grid_get_index(g,index - stride); 411 | if (value_after == HUGE_VAL || value_before == HUGE_VAL) 412 | { *valuep = HUGE_VAL; return 0; } 413 | diff = value_after; 414 | diff -= value_before; 415 | goto save_continue; 416 | use_next: 417 | /* Use next cell for gradient */ 418 | value_after = *(double *)cd_grid_get_index(g,index + stride); 419 | value_before = *(double *)cd_grid_get_index(g,index); 420 | if (value_after == HUGE_VAL || value_before == HUGE_VAL) 421 | { *valuep = HUGE_VAL; return 0; } 422 | diff = value_after; 423 | diff -= value_before; 424 | goto save_continue; 425 | /* Save to the grad vector, accumulate the stride */ 426 | save_continue: 427 | /* Adjust the value based on the gradient */ 428 | grad = diff * g->sizes[ni] / g->lengths[ni]; 429 | value += grad * (p[ni] - center); 430 | stride *= g->sizes[ni]; 431 | } 432 | 433 | *valuep = value; 434 | return 0; 435 | } 436 | 437 | 438 | int cd_grid_double_sedt(struct cd_grid ** gp_dt, struct cd_grid * g_func) 439 | { 440 | return cd_grid_double_dt_sqeuc(gp_dt, g_func); 441 | } 442 | 443 | int cd_grid_double_dt_sqeuc(struct cd_grid ** gp_dt, struct cd_grid * g_func) 444 | { 445 | int i; 446 | int ret; 447 | int n; 448 | int err; 449 | int ni; 450 | struct cd_grid * g; 451 | int * subs; 452 | int ni2; 453 | int * v; 454 | double * z; 455 | double * func; 456 | int dim_n; 457 | int dim_stride; 458 | double dim_res2; 459 | double * trans; 460 | 461 | ret = 0; 462 | 463 | n = g_func->n; 464 | 465 | subs = 0; 466 | v = 0; 467 | z = 0; 468 | func = 0; 469 | 470 | /* Allocate space to keep n+1 subscripts 471 | * (the msb flips from 0 to 1 when we're done) */ 472 | subs = (int *) malloc((n+1) * sizeof(int)); 473 | if (!subs) { ret = -1; goto error; } 474 | 475 | /* Start with copy of sampled function */ 476 | err = cd_grid_create_copy(&g, g_func); 477 | if (err) { ret = -1; goto error; } 478 | 479 | /* Operate the 1-dimensional squared euclidean distance transform 480 | * over each dimension in turn, updating the values for each slice */ 481 | for (ni=0; nisizes[ni]; 488 | 489 | /* Calculate slice stride (product of all smaller dimension sizes) */ 490 | dim_stride = 1; 491 | for (ni2=ni+1; ni2sizes[ni2]; 493 | 494 | /* Grab extent for this dimension */ 495 | dim_res2 = pow(g_func->lengths[ni]/g->sizes[ni], 2.0); 496 | 497 | /* Set up temp arrays for the 1d transform */ 498 | v = (int *) malloc(dim_n * sizeof(int)); 499 | z = (double *) malloc((dim_n + 1) * sizeof(double)); 500 | func = (double *) malloc(dim_n*sizeof(double)); 501 | if (!v || !z || !func) { ret = -1; goto error; } 502 | 503 | /* Loop until we're done (the msb subscript flips) */ 504 | while (subs[n]==0) 505 | { 506 | /* Get slice data pointer (first element) */ 507 | trans = (double *)cd_grid_get_subs(g, subs); 508 | 509 | /* Perform 1d transform (scale by extent) */ 510 | /* XXX BUG: 511 | * Scaling HUGE_VAL is not guarenteed to maintain HUGE_VAL 512 | * (e.g. on systems with no IEEE infinitiy) */ 513 | for (i=0; isizes[ni2])) 523 | { 524 | subs[ni2+1]++; 525 | subs[ni2] = 0; 526 | } 527 | } 528 | 529 | /* Continue! */ 530 | } 531 | 532 | free(v); 533 | free(z); 534 | free(func); 535 | v = 0; 536 | z = 0; 537 | func = 0; 538 | } 539 | 540 | error: 541 | free(func); 542 | free(v); 543 | free(z); 544 | free(subs); 545 | if (ret == 0) 546 | *gp_dt = g; 547 | else if (g) 548 | cd_grid_destroy(g); 549 | return ret; 550 | } 551 | 552 | int cd_grid_double_dt_sgneuc(struct cd_grid ** gp_dt, struct cd_grid * g_binobs) 553 | { 554 | int ret; 555 | int err; 556 | struct cd_grid * g_vox_emp = 0; 557 | struct cd_grid * g_vox_obs = 0; 558 | struct cd_grid * g_sedt_emp = 0; 559 | struct cd_grid * g_sedt_obs = 0; 560 | struct cd_grid * g_dt = 0; 561 | double dval; 562 | size_t index; 563 | 564 | if (g_binobs->cell_size != sizeof(char)) 565 | return -2; 566 | 567 | ret = 0; 568 | 569 | /* g_vox_emp is 0.0 in free space, HUGE_VAL elsewhere */ 570 | dval = 0.0; 571 | err = cd_grid_create_sizearray(&g_vox_emp, &dval, sizeof(dval), g_binobs->n, g_binobs->sizes); 572 | if (err) { ret = -1; goto error; } 573 | err = cd_grid_create_sizearray(&g_vox_obs, &dval, sizeof(dval), g_binobs->n, g_binobs->sizes); 574 | if (err) { ret = -1; goto error; } 575 | memcpy(g_vox_emp->lengths, g_binobs->lengths, g_binobs->n*sizeof(double)); 576 | memcpy(g_vox_emp->lengths, g_binobs->lengths, g_binobs->n*sizeof(double)); 577 | 578 | /* g_vox_obs is 0.0 in obstacles, HUGE_VAL elsewhere */ 579 | for (index=0; indexncells; index++) 580 | { 581 | if (*(char *)cd_grid_get_index(g_binobs,index)) /* in obstacle */ 582 | { 583 | *(double *)cd_grid_get_index(g_vox_emp,index) = HUGE_VAL; 584 | *(double *)cd_grid_get_index(g_vox_obs,index) = 0.0; 585 | } 586 | else /* free space */ 587 | { 588 | *(double *)cd_grid_get_index(g_vox_emp,index) = 0.0; 589 | *(double *)cd_grid_get_index(g_vox_obs,index) = HUGE_VAL; 590 | } 591 | } 592 | 593 | /* Compute the Squared Euclidean Distance Transform of each voxel grid */ 594 | err = cd_grid_double_sedt(&g_sedt_emp, g_vox_emp); 595 | if (err) { ret = -1; goto error; } 596 | err = cd_grid_double_sedt(&g_sedt_obs, g_vox_obs); 597 | if (err) { ret = -1; goto error; } 598 | 599 | /* Create signed distance field grid (obs-emp) */ 600 | err = cd_grid_create_copy(&g_dt, g_sedt_obs); 601 | if (err) { ret = -1; goto error; } 602 | for (index=0; indexncells; index++) 603 | { 604 | *(double *)cd_grid_get_index(g_dt, index) = 605 | sqrt(*(double *)cd_grid_get_index(g_dt, index)) - 606 | sqrt(*(double *)cd_grid_get_index(g_sedt_emp, index)); 607 | } 608 | 609 | error: 610 | cd_grid_destroy(g_vox_emp); 611 | cd_grid_destroy(g_vox_obs); 612 | cd_grid_destroy(g_sedt_emp); 613 | cd_grid_destroy(g_sedt_obs); 614 | if (ret == 0) *gp_dt = g_dt; 615 | return ret; 616 | } 617 | 618 | int cd_grid_double_bin_sdf(struct cd_grid ** gp_dt, struct cd_grid * g_emp) 619 | { 620 | int ret; 621 | int err; 622 | struct cd_grid * g_vox_emp; 623 | struct cd_grid * g_vox_obs = 0; 624 | struct cd_grid * g_sedt_emp = 0; 625 | struct cd_grid * g_sedt_obs = 0; 626 | struct cd_grid * g_dt = 0; 627 | size_t index; 628 | 629 | if (g_emp->cell_size != sizeof(double)) 630 | return -2; 631 | 632 | ret = 0; 633 | 634 | /* g_vox_emp is 0.0 in free space, HUGE_VAL elsewhere */ 635 | g_vox_emp = g_emp; 636 | 637 | /* g_vox_obs is 0.0 in obstacles, HUGE_VAL elsewhere */ 638 | err = cd_grid_create_copy(&g_vox_obs, g_vox_emp); 639 | if (err) { ret = -1; goto error; } 640 | for (index=0; indexncells; index++) 641 | { 642 | *(double *)cd_grid_get_index(g_vox_obs,index) = 643 | (*(double *)cd_grid_get_index(g_vox_emp,index) == 0.0) ? HUGE_VAL : 0.0; 644 | } 645 | 646 | /* Compute the Squared Euclidean Distance Transform of each voxel grid */ 647 | err = cd_grid_double_sedt(&g_sedt_emp, g_vox_emp); 648 | if (err) { ret = -1; goto error; } 649 | err = cd_grid_double_sedt(&g_sedt_obs, g_vox_obs); 650 | if (err) { ret = -1; goto error; } 651 | 652 | /* Create signed distance field grid (obs-emp) */ 653 | err = cd_grid_create_copy(&g_dt, g_sedt_obs); 654 | if (err) { ret = -1; goto error; } 655 | for (index=0; indexncells; index++) 656 | { 657 | *(double *)cd_grid_get_index(g_dt, index) = 658 | sqrt(*(double *)cd_grid_get_index(g_dt, index)) - 659 | sqrt(*(double *)cd_grid_get_index(g_sedt_emp, index)); 660 | } 661 | 662 | error: 663 | cd_grid_destroy(g_vox_obs); 664 | cd_grid_destroy(g_sedt_emp); 665 | cd_grid_destroy(g_sedt_obs); 666 | if (ret == 0) *gp_dt = g_dt; 667 | return ret; 668 | } -------------------------------------------------------------------------------- /examples/data/envs/stl/table.stl: -------------------------------------------------------------------------------- 1 | solid CATIA STL 2 | facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 3 | outer loop 4 | vertex -7.135000e-001 -2.575000e-001 0.000000e+000 5 | vertex -8.135000e-001 -2.575000e-001 0.000000e+000 6 | vertex -8.135000e-001 2.575000e-001 0.000000e+000 7 | endloop 8 | endfacet 9 | facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 10 | outer loop 11 | vertex -7.135000e-001 -2.575000e-001 0.000000e+000 12 | vertex -8.135000e-001 2.575000e-001 0.000000e+000 13 | vertex -7.135000e-001 2.575000e-001 0.000000e+000 14 | endloop 15 | endfacet 16 | facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 17 | outer loop 18 | vertex -7.135000e-001 -2.575000e-001 0.000000e+000 19 | vertex -7.135000e-001 2.575000e-001 0.000000e+000 20 | vertex 7.135000e-001 2.575000e-001 0.000000e+000 21 | endloop 22 | endfacet 23 | facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 24 | outer loop 25 | vertex -7.135000e-001 -2.575000e-001 0.000000e+000 26 | vertex 7.135000e-001 2.575000e-001 0.000000e+000 27 | vertex 7.135000e-001 -2.575000e-001 0.000000e+000 28 | endloop 29 | endfacet 30 | facet normal -0.000000e+000 -0.000000e+000 -1.000000e+000 31 | outer loop 32 | vertex -7.135000e-001 -2.575000e-001 0.000000e+000 33 | vertex 7.135000e-001 -2.575000e-001 0.000000e+000 34 | vertex 7.135000e-001 -3.575000e-001 0.000000e+000 35 | endloop 36 | endfacet 37 | facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 38 | outer loop 39 | vertex -7.135000e-001 -2.575000e-001 0.000000e+000 40 | vertex 7.135000e-001 -3.575000e-001 0.000000e+000 41 | vertex -7.135000e-001 -3.575000e-001 0.000000e+000 42 | endloop 43 | endfacet 44 | facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 45 | outer loop 46 | vertex -9.135001e-001 4.575000e-001 0.000000e+000 47 | vertex 9.135001e-001 4.575000e-001 0.000000e+000 48 | vertex 8.135000e-001 3.575000e-001 0.000000e+000 49 | endloop 50 | endfacet 51 | facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 52 | outer loop 53 | vertex -9.135001e-001 4.575000e-001 0.000000e+000 54 | vertex 8.135000e-001 3.575000e-001 0.000000e+000 55 | vertex 7.135000e-001 3.575000e-001 0.000000e+000 56 | endloop 57 | endfacet 58 | facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 59 | outer loop 60 | vertex -9.135001e-001 4.575000e-001 0.000000e+000 61 | vertex 7.135000e-001 3.575000e-001 0.000000e+000 62 | vertex -7.135000e-001 3.575000e-001 0.000000e+000 63 | endloop 64 | endfacet 65 | facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 66 | outer loop 67 | vertex -9.135001e-001 4.575000e-001 0.000000e+000 68 | vertex -7.135000e-001 3.575000e-001 0.000000e+000 69 | vertex -8.135000e-001 3.575000e-001 0.000000e+000 70 | endloop 71 | endfacet 72 | facet normal -0.000000e+000 -0.000000e+000 -1.000000e+000 73 | outer loop 74 | vertex -9.135001e-001 4.575000e-001 0.000000e+000 75 | vertex -8.135000e-001 3.575000e-001 0.000000e+000 76 | vertex -8.135000e-001 2.575000e-001 0.000000e+000 77 | endloop 78 | endfacet 79 | facet normal -0.000000e+000 -0.000000e+000 -1.000000e+000 80 | outer loop 81 | vertex -9.135001e-001 4.575000e-001 0.000000e+000 82 | vertex -8.135000e-001 2.575000e-001 0.000000e+000 83 | vertex -8.135000e-001 -2.575000e-001 0.000000e+000 84 | endloop 85 | endfacet 86 | facet normal -0.000000e+000 -0.000000e+000 -1.000000e+000 87 | outer loop 88 | vertex -9.135001e-001 4.575000e-001 0.000000e+000 89 | vertex -8.135000e-001 -2.575000e-001 0.000000e+000 90 | vertex -8.135000e-001 -3.575000e-001 0.000000e+000 91 | endloop 92 | endfacet 93 | facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 94 | outer loop 95 | vertex -9.135001e-001 4.575000e-001 0.000000e+000 96 | vertex -8.135000e-001 -3.575000e-001 0.000000e+000 97 | vertex -9.135001e-001 -4.575000e-001 0.000000e+000 98 | endloop 99 | endfacet 100 | facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 101 | outer loop 102 | vertex 9.135001e-001 4.575000e-001 0.000000e+000 103 | vertex 9.135001e-001 -4.575000e-001 0.000000e+000 104 | vertex 8.135000e-001 -3.575000e-001 0.000000e+000 105 | endloop 106 | endfacet 107 | facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 108 | outer loop 109 | vertex 9.135001e-001 4.575000e-001 0.000000e+000 110 | vertex 8.135000e-001 -3.575000e-001 0.000000e+000 111 | vertex 8.135000e-001 -2.575000e-001 0.000000e+000 112 | endloop 113 | endfacet 114 | facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 115 | outer loop 116 | vertex 9.135001e-001 4.575000e-001 0.000000e+000 117 | vertex 8.135000e-001 -2.575000e-001 0.000000e+000 118 | vertex 8.135000e-001 2.575000e-001 0.000000e+000 119 | endloop 120 | endfacet 121 | facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 122 | outer loop 123 | vertex 9.135001e-001 4.575000e-001 0.000000e+000 124 | vertex 8.135000e-001 2.575000e-001 0.000000e+000 125 | vertex 8.135000e-001 3.575000e-001 0.000000e+000 126 | endloop 127 | endfacet 128 | facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 129 | outer loop 130 | vertex -9.135001e-001 -4.575000e-001 0.000000e+000 131 | vertex -8.135000e-001 -3.575000e-001 0.000000e+000 132 | vertex -7.135000e-001 -3.575000e-001 0.000000e+000 133 | endloop 134 | endfacet 135 | facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 136 | outer loop 137 | vertex -9.135001e-001 -4.575000e-001 0.000000e+000 138 | vertex -7.135000e-001 -3.575000e-001 0.000000e+000 139 | vertex 7.135000e-001 -3.575000e-001 0.000000e+000 140 | endloop 141 | endfacet 142 | facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 143 | outer loop 144 | vertex -9.135001e-001 -4.575000e-001 0.000000e+000 145 | vertex 7.135000e-001 -3.575000e-001 0.000000e+000 146 | vertex 8.135000e-001 -3.575000e-001 0.000000e+000 147 | endloop 148 | endfacet 149 | facet normal 0.000000e+000 -0.000000e+000 -1.000000e+000 150 | outer loop 151 | vertex -9.135001e-001 -4.575000e-001 0.000000e+000 152 | vertex 8.135000e-001 -3.575000e-001 0.000000e+000 153 | vertex 9.135001e-001 -4.575000e-001 0.000000e+000 154 | endloop 155 | endfacet 156 | facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 157 | outer loop 158 | vertex 7.135000e-001 -2.575000e-001 0.000000e+000 159 | vertex 7.135000e-001 2.575000e-001 0.000000e+000 160 | vertex 8.135000e-001 -2.575000e-001 0.000000e+000 161 | endloop 162 | endfacet 163 | facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 164 | outer loop 165 | vertex 8.135000e-001 -2.575000e-001 0.000000e+000 166 | vertex 7.135000e-001 2.575000e-001 0.000000e+000 167 | vertex 8.135000e-001 2.575000e-001 0.000000e+000 168 | endloop 169 | endfacet 170 | facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 171 | outer loop 172 | vertex 7.135000e-001 2.575000e-001 0.000000e+000 173 | vertex -7.135000e-001 2.575000e-001 0.000000e+000 174 | vertex 7.135000e-001 3.575000e-001 0.000000e+000 175 | endloop 176 | endfacet 177 | facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 178 | outer loop 179 | vertex 7.135000e-001 3.575000e-001 0.000000e+000 180 | vertex -7.135000e-001 2.575000e-001 0.000000e+000 181 | vertex -7.135000e-001 3.575000e-001 0.000000e+000 182 | endloop 183 | endfacet 184 | facet normal 0.000000e+000 1.000000e+000 0.000000e+000 185 | outer loop 186 | vertex 7.135000e-001 3.575000e-001 -8.130000e-001 187 | vertex 7.135000e-001 3.575000e-001 0.000000e+000 188 | vertex 8.135000e-001 3.575000e-001 -8.130000e-001 189 | endloop 190 | endfacet 191 | facet normal 0.000000e+000 1.000000e+000 -0.000000e+000 192 | outer loop 193 | vertex 8.135000e-001 3.575000e-001 -8.130000e-001 194 | vertex 7.135000e-001 3.575000e-001 0.000000e+000 195 | vertex 8.135000e-001 3.575000e-001 0.000000e+000 196 | endloop 197 | endfacet 198 | facet normal -1.000000e+000 -0.000000e+000 0.000000e+000 199 | outer loop 200 | vertex 7.135000e-001 2.575000e-001 0.000000e+000 201 | vertex 7.135000e-001 3.575000e-001 0.000000e+000 202 | vertex 7.135000e-001 2.575000e-001 -8.130000e-001 203 | endloop 204 | endfacet 205 | facet normal -1.000000e+000 0.000000e+000 0.000000e+000 206 | outer loop 207 | vertex 7.135000e-001 2.575000e-001 -8.130000e-001 208 | vertex 7.135000e-001 3.575000e-001 0.000000e+000 209 | vertex 7.135000e-001 3.575000e-001 -8.130000e-001 210 | endloop 211 | endfacet 212 | facet normal 0.000000e+000 -1.000000e+000 -0.000000e+000 213 | outer loop 214 | vertex 8.135000e-001 2.575000e-001 -8.130000e-001 215 | vertex 8.135000e-001 2.575000e-001 0.000000e+000 216 | vertex 7.135000e-001 2.575000e-001 -8.130000e-001 217 | endloop 218 | endfacet 219 | facet normal 0.000000e+000 -1.000000e+000 0.000000e+000 220 | outer loop 221 | vertex 7.135000e-001 2.575000e-001 -8.130000e-001 222 | vertex 8.135000e-001 2.575000e-001 0.000000e+000 223 | vertex 7.135000e-001 2.575000e-001 0.000000e+000 224 | endloop 225 | endfacet 226 | facet normal 1.000000e+000 -0.000000e+000 0.000000e+000 227 | outer loop 228 | vertex 8.135000e-001 3.575000e-001 0.000000e+000 229 | vertex 8.135000e-001 2.575000e-001 0.000000e+000 230 | vertex 8.135000e-001 3.575000e-001 -8.130000e-001 231 | endloop 232 | endfacet 233 | facet normal 1.000000e+000 0.000000e+000 0.000000e+000 234 | outer loop 235 | vertex 8.135000e-001 3.575000e-001 -8.130000e-001 236 | vertex 8.135000e-001 2.575000e-001 0.000000e+000 237 | vertex 8.135000e-001 2.575000e-001 -8.130000e-001 238 | endloop 239 | endfacet 240 | facet normal -1.000000e+000 -0.000000e+000 0.000000e+000 241 | outer loop 242 | vertex -8.135000e-001 2.575000e-001 0.000000e+000 243 | vertex -8.135000e-001 3.575000e-001 0.000000e+000 244 | vertex -8.135000e-001 2.575000e-001 -8.130000e-001 245 | endloop 246 | endfacet 247 | facet normal -1.000000e+000 0.000000e+000 0.000000e+000 248 | outer loop 249 | vertex -8.135000e-001 2.575000e-001 -8.130000e-001 250 | vertex -8.135000e-001 3.575000e-001 0.000000e+000 251 | vertex -8.135000e-001 3.575000e-001 -8.130000e-001 252 | endloop 253 | endfacet 254 | facet normal 0.000000e+000 -1.000000e+000 -0.000000e+000 255 | outer loop 256 | vertex -7.135000e-001 2.575000e-001 -8.130000e-001 257 | vertex -7.135000e-001 2.575000e-001 0.000000e+000 258 | vertex -8.135000e-001 2.575000e-001 -8.130000e-001 259 | endloop 260 | endfacet 261 | facet normal 0.000000e+000 -1.000000e+000 0.000000e+000 262 | outer loop 263 | vertex -8.135000e-001 2.575000e-001 -8.130000e-001 264 | vertex -7.135000e-001 2.575000e-001 0.000000e+000 265 | vertex -8.135000e-001 2.575000e-001 0.000000e+000 266 | endloop 267 | endfacet 268 | facet normal 1.000000e+000 -0.000000e+000 0.000000e+000 269 | outer loop 270 | vertex -7.135000e-001 3.575000e-001 0.000000e+000 271 | vertex -7.135000e-001 2.575000e-001 0.000000e+000 272 | vertex -7.135000e-001 3.575000e-001 -8.130000e-001 273 | endloop 274 | endfacet 275 | facet normal 1.000000e+000 0.000000e+000 0.000000e+000 276 | outer loop 277 | vertex -7.135000e-001 3.575000e-001 -8.130000e-001 278 | vertex -7.135000e-001 2.575000e-001 0.000000e+000 279 | vertex -7.135000e-001 2.575000e-001 -8.130000e-001 280 | endloop 281 | endfacet 282 | facet normal 0.000000e+000 1.000000e+000 0.000000e+000 283 | outer loop 284 | vertex -8.135000e-001 3.575000e-001 -8.130000e-001 285 | vertex -8.135000e-001 3.575000e-001 0.000000e+000 286 | vertex -7.135000e-001 3.575000e-001 -8.130000e-001 287 | endloop 288 | endfacet 289 | facet normal 0.000000e+000 1.000000e+000 -0.000000e+000 290 | outer loop 291 | vertex -7.135000e-001 3.575000e-001 -8.130000e-001 292 | vertex -8.135000e-001 3.575000e-001 0.000000e+000 293 | vertex -7.135000e-001 3.575000e-001 0.000000e+000 294 | endloop 295 | endfacet 296 | facet normal 1.000000e+000 -0.000000e+000 0.000000e+000 297 | outer loop 298 | vertex 8.135000e-001 -2.575000e-001 0.000000e+000 299 | vertex 8.135000e-001 -3.575000e-001 0.000000e+000 300 | vertex 8.135000e-001 -2.575000e-001 -8.130000e-001 301 | endloop 302 | endfacet 303 | facet normal 1.000000e+000 0.000000e+000 0.000000e+000 304 | outer loop 305 | vertex 8.135000e-001 -2.575000e-001 -8.130000e-001 306 | vertex 8.135000e-001 -3.575000e-001 0.000000e+000 307 | vertex 8.135000e-001 -3.575000e-001 -8.130000e-001 308 | endloop 309 | endfacet 310 | facet normal 0.000000e+000 1.000000e+000 -0.000000e+000 311 | outer loop 312 | vertex 8.135000e-001 -2.575000e-001 0.000000e+000 313 | vertex 8.135000e-001 -2.575000e-001 -8.130000e-001 314 | vertex 7.135000e-001 -2.575000e-001 0.000000e+000 315 | endloop 316 | endfacet 317 | facet normal 0.000000e+000 1.000000e+000 0.000000e+000 318 | outer loop 319 | vertex 7.135000e-001 -2.575000e-001 0.000000e+000 320 | vertex 8.135000e-001 -2.575000e-001 -8.130000e-001 321 | vertex 7.135000e-001 -2.575000e-001 -8.130000e-001 322 | endloop 323 | endfacet 324 | facet normal -1.000000e+000 -0.000000e+000 0.000000e+000 325 | outer loop 326 | vertex 7.135000e-001 -3.575000e-001 0.000000e+000 327 | vertex 7.135000e-001 -2.575000e-001 0.000000e+000 328 | vertex 7.135000e-001 -3.575000e-001 -8.130000e-001 329 | endloop 330 | endfacet 331 | facet normal -1.000000e+000 0.000000e+000 0.000000e+000 332 | outer loop 333 | vertex 7.135000e-001 -3.575000e-001 -8.130000e-001 334 | vertex 7.135000e-001 -2.575000e-001 0.000000e+000 335 | vertex 7.135000e-001 -2.575000e-001 -8.130000e-001 336 | endloop 337 | endfacet 338 | facet normal 0.000000e+000 -1.000000e+000 0.000000e+000 339 | outer loop 340 | vertex 7.135000e-001 -3.575000e-001 0.000000e+000 341 | vertex 7.135000e-001 -3.575000e-001 -8.130000e-001 342 | vertex 8.135000e-001 -3.575000e-001 0.000000e+000 343 | endloop 344 | endfacet 345 | facet normal 0.000000e+000 -1.000000e+000 -0.000000e+000 346 | outer loop 347 | vertex 8.135000e-001 -3.575000e-001 0.000000e+000 348 | vertex 7.135000e-001 -3.575000e-001 -8.130000e-001 349 | vertex 8.135000e-001 -3.575000e-001 -8.130000e-001 350 | endloop 351 | endfacet 352 | facet normal 0.000000e+000 -1.000000e+000 0.000000e+000 353 | outer loop 354 | vertex -8.135000e-001 -3.575000e-001 0.000000e+000 355 | vertex -8.135000e-001 -3.575000e-001 -8.130000e-001 356 | vertex -7.135000e-001 -3.575000e-001 0.000000e+000 357 | endloop 358 | endfacet 359 | facet normal 0.000000e+000 -1.000000e+000 -0.000000e+000 360 | outer loop 361 | vertex -7.135000e-001 -3.575000e-001 0.000000e+000 362 | vertex -8.135000e-001 -3.575000e-001 -8.130000e-001 363 | vertex -7.135000e-001 -3.575000e-001 -8.130000e-001 364 | endloop 365 | endfacet 366 | facet normal 1.000000e+000 -0.000000e+000 0.000000e+000 367 | outer loop 368 | vertex -7.135000e-001 -2.575000e-001 0.000000e+000 369 | vertex -7.135000e-001 -3.575000e-001 0.000000e+000 370 | vertex -7.135000e-001 -2.575000e-001 -8.130000e-001 371 | endloop 372 | endfacet 373 | facet normal 1.000000e+000 0.000000e+000 0.000000e+000 374 | outer loop 375 | vertex -7.135000e-001 -2.575000e-001 -8.130000e-001 376 | vertex -7.135000e-001 -3.575000e-001 0.000000e+000 377 | vertex -7.135000e-001 -3.575000e-001 -8.130000e-001 378 | endloop 379 | endfacet 380 | facet normal 0.000000e+000 1.000000e+000 -0.000000e+000 381 | outer loop 382 | vertex -7.135000e-001 -2.575000e-001 0.000000e+000 383 | vertex -7.135000e-001 -2.575000e-001 -8.130000e-001 384 | vertex -8.135000e-001 -2.575000e-001 0.000000e+000 385 | endloop 386 | endfacet 387 | facet normal 0.000000e+000 1.000000e+000 0.000000e+000 388 | outer loop 389 | vertex -8.135000e-001 -2.575000e-001 0.000000e+000 390 | vertex -7.135000e-001 -2.575000e-001 -8.130000e-001 391 | vertex -8.135000e-001 -2.575000e-001 -8.130000e-001 392 | endloop 393 | endfacet 394 | facet normal -1.000000e+000 -0.000000e+000 0.000000e+000 395 | outer loop 396 | vertex -8.135000e-001 -3.575000e-001 0.000000e+000 397 | vertex -8.135000e-001 -2.575000e-001 0.000000e+000 398 | vertex -8.135000e-001 -3.575000e-001 -8.130000e-001 399 | endloop 400 | endfacet 401 | facet normal -1.000000e+000 0.000000e+000 0.000000e+000 402 | outer loop 403 | vertex -8.135000e-001 -3.575000e-001 -8.130000e-001 404 | vertex -8.135000e-001 -2.575000e-001 0.000000e+000 405 | vertex -8.135000e-001 -2.575000e-001 -8.130000e-001 406 | endloop 407 | endfacet 408 | facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 409 | outer loop 410 | vertex 7.135000e-001 3.575000e-001 -8.130000e-001 411 | vertex 8.135000e-001 3.575000e-001 -8.130000e-001 412 | vertex 7.135000e-001 2.575000e-001 -8.130000e-001 413 | endloop 414 | endfacet 415 | facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 416 | outer loop 417 | vertex 7.135000e-001 2.575000e-001 -8.130000e-001 418 | vertex 8.135000e-001 3.575000e-001 -8.130000e-001 419 | vertex 8.135000e-001 2.575000e-001 -8.130000e-001 420 | endloop 421 | endfacet 422 | facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 423 | outer loop 424 | vertex -8.135000e-001 3.575000e-001 -8.130000e-001 425 | vertex -7.135000e-001 3.575000e-001 -8.130000e-001 426 | vertex -8.135000e-001 2.575000e-001 -8.130000e-001 427 | endloop 428 | endfacet 429 | facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 430 | outer loop 431 | vertex -8.135000e-001 2.575000e-001 -8.130000e-001 432 | vertex -7.135000e-001 3.575000e-001 -8.130000e-001 433 | vertex -7.135000e-001 2.575000e-001 -8.130000e-001 434 | endloop 435 | endfacet 436 | facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 437 | outer loop 438 | vertex 7.135000e-001 -2.575000e-001 -8.130000e-001 439 | vertex 8.135000e-001 -2.575000e-001 -8.130000e-001 440 | vertex 7.135000e-001 -3.575000e-001 -8.130000e-001 441 | endloop 442 | endfacet 443 | facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 444 | outer loop 445 | vertex 7.135000e-001 -3.575000e-001 -8.130000e-001 446 | vertex 8.135000e-001 -2.575000e-001 -8.130000e-001 447 | vertex 8.135000e-001 -3.575000e-001 -8.130000e-001 448 | endloop 449 | endfacet 450 | facet normal -0.000000e+000 0.000000e+000 -1.000000e+000 451 | outer loop 452 | vertex -8.135000e-001 -2.575000e-001 -8.130000e-001 453 | vertex -7.135000e-001 -2.575000e-001 -8.130000e-001 454 | vertex -8.135000e-001 -3.575000e-001 -8.130000e-001 455 | endloop 456 | endfacet 457 | facet normal 0.000000e+000 0.000000e+000 -1.000000e+000 458 | outer loop 459 | vertex -8.135000e-001 -3.575000e-001 -8.130000e-001 460 | vertex -7.135000e-001 -2.575000e-001 -8.130000e-001 461 | vertex -7.135000e-001 -3.575000e-001 -8.130000e-001 462 | endloop 463 | endfacet 464 | facet normal 0.000000e+000 0.000000e+000 1.000000e+000 465 | outer loop 466 | vertex -9.135001e-001 -4.575000e-001 4.200000e-002 467 | vertex 9.135001e-001 -4.575000e-001 4.200000e-002 468 | vertex -9.135001e-001 4.575000e-001 4.200000e-002 469 | endloop 470 | endfacet 471 | facet normal -0.000000e+000 0.000000e+000 1.000000e+000 472 | outer loop 473 | vertex -9.135001e-001 4.575000e-001 4.200000e-002 474 | vertex 9.135001e-001 -4.575000e-001 4.200000e-002 475 | vertex 9.135001e-001 4.575000e-001 4.200000e-002 476 | endloop 477 | endfacet 478 | facet normal 0.000000e+000 1.000000e+000 0.000000e+000 479 | outer loop 480 | vertex -9.135001e-001 4.575000e-001 0.000000e+000 481 | vertex -9.135001e-001 4.575000e-001 4.200000e-002 482 | vertex 9.135001e-001 4.575000e-001 0.000000e+000 483 | endloop 484 | endfacet 485 | facet normal 0.000000e+000 1.000000e+000 -0.000000e+000 486 | outer loop 487 | vertex 9.135001e-001 4.575000e-001 0.000000e+000 488 | vertex -9.135001e-001 4.575000e-001 4.200000e-002 489 | vertex 9.135001e-001 4.575000e-001 4.200000e-002 490 | endloop 491 | endfacet 492 | facet normal 1.000000e+000 0.000000e+000 0.000000e+000 493 | outer loop 494 | vertex 9.135001e-001 -4.575000e-001 4.200000e-002 495 | vertex 9.135001e-001 -4.575000e-001 0.000000e+000 496 | vertex 9.135001e-001 4.575000e-001 4.200000e-002 497 | endloop 498 | endfacet 499 | facet normal 1.000000e+000 0.000000e+000 0.000000e+000 500 | outer loop 501 | vertex 9.135001e-001 4.575000e-001 4.200000e-002 502 | vertex 9.135001e-001 -4.575000e-001 0.000000e+000 503 | vertex 9.135001e-001 4.575000e-001 0.000000e+000 504 | endloop 505 | endfacet 506 | facet normal 0.000000e+000 -1.000000e+000 -0.000000e+000 507 | outer loop 508 | vertex 9.135001e-001 -4.575000e-001 0.000000e+000 509 | vertex 9.135001e-001 -4.575000e-001 4.200000e-002 510 | vertex -9.135001e-001 -4.575000e-001 0.000000e+000 511 | endloop 512 | endfacet 513 | facet normal 0.000000e+000 -1.000000e+000 0.000000e+000 514 | outer loop 515 | vertex -9.135001e-001 -4.575000e-001 0.000000e+000 516 | vertex 9.135001e-001 -4.575000e-001 4.200000e-002 517 | vertex -9.135001e-001 -4.575000e-001 4.200000e-002 518 | endloop 519 | endfacet 520 | facet normal -1.000000e+000 0.000000e+000 0.000000e+000 521 | outer loop 522 | vertex -9.135001e-001 4.575000e-001 4.200000e-002 523 | vertex -9.135001e-001 4.575000e-001 0.000000e+000 524 | vertex -9.135001e-001 -4.575000e-001 4.200000e-002 525 | endloop 526 | endfacet 527 | facet normal -1.000000e+000 0.000000e+000 0.000000e+000 528 | outer loop 529 | vertex -9.135001e-001 -4.575000e-001 4.200000e-002 530 | vertex -9.135001e-001 4.575000e-001 0.000000e+000 531 | vertex -9.135001e-001 -4.575000e-001 0.000000e+000 532 | endloop 533 | endfacet 534 | endsolid CATIA STL 535 | -------------------------------------------------------------------------------- /src/utils/kin.c: -------------------------------------------------------------------------------- 1 | /** 2 | * \file kin.c 3 | * \brief Implementation of cd_kin, a collection of useful routines for 4 | * kinematics. 5 | */ 6 | 7 | #include 8 | #include 9 | #include "mat.h" 10 | #include "kin.h" 11 | 12 | #define TAU (6.2831853071795864769252867665590057683943387987502116) 13 | 14 | int cd_kin_quat_identity(double quat[4]) 15 | { 16 | quat[0] = 0.0; 17 | quat[1] = 0.0; 18 | quat[2] = 0.0; 19 | quat[3] = 1.0; 20 | return 0; 21 | } 22 | 23 | int cd_kin_pose_identity(double pose[7]) 24 | { 25 | pose[0] = 0.0; 26 | pose[1] = 0.0; 27 | pose[2] = 0.0; 28 | pose[3] = 0.0; 29 | pose[4] = 0.0; 30 | pose[5] = 0.0; 31 | pose[6] = 1.0; 32 | return 0; 33 | } 34 | 35 | /* Note - should we check for len = 0.0? */ 36 | int cd_kin_quat_normalize(double quat[4]) 37 | { 38 | double len; 39 | len = cblas_dnrm2(4, quat, 1); 40 | cblas_dscal(4, 1.0/len, quat, 1); 41 | return 0; 42 | } 43 | 44 | /* Note - should we check for len = 0.0? */ 45 | int cd_kin_pose_normalize(double pose[7]) 46 | { 47 | double len; 48 | len = cblas_dnrm2(4, pose+3, 1); 49 | cblas_dscal(4, 1.0/len, pose+3, 1); 50 | return 0; 51 | } 52 | 53 | int cd_kin_quat_flip_closerto(double quat[4], const double target[4]) 54 | { 55 | int i; 56 | double orig_diff; 57 | double flipped_diff; 58 | double orig_sumsq = 0.0; 59 | double flipped_sumsq = 0.0; 60 | for (i=0; i<4; i++) 61 | { 62 | orig_diff = quat[i] - target[i]; 63 | flipped_diff = - quat[i] - target[i]; 64 | orig_sumsq += orig_diff * orig_diff; 65 | flipped_sumsq += flipped_diff * flipped_diff; 66 | } 67 | if (flipped_sumsq < orig_sumsq) 68 | { 69 | for (i=0; i<4; i++) 70 | quat[i] *= -1.0; 71 | } 72 | return 0; 73 | } 74 | 75 | int cd_kin_pose_flip_closerto(double pose[7], const double target[7]) 76 | { 77 | int i; 78 | double orig_diff; 79 | double flipped_diff; 80 | double orig_sumsq = 0.0; 81 | double flipped_sumsq = 0.0; 82 | for (i=3; i<7; i++) 83 | { 84 | orig_diff = pose[i] - target[i]; 85 | flipped_diff = - pose[i] - target[i]; 86 | orig_sumsq += orig_diff * orig_diff; 87 | flipped_sumsq += flipped_diff * flipped_diff; 88 | } 89 | if (flipped_sumsq < orig_sumsq) 90 | { 91 | for (i=3; i<7; i++) 92 | pose[i] *= -1.0; 93 | } 94 | return 0; 95 | } 96 | 97 | int cd_kin_quat_compose(const double quat_ab[4], const double quat_bc[4], double quat_ac[4]) 98 | { 99 | double qabx, qaby, qabz, qabw; 100 | double qbcx, qbcy, qbcz, qbcw; 101 | qabx = quat_ab[0]; 102 | qaby = quat_ab[1]; 103 | qabz = quat_ab[2]; 104 | qabw = quat_ab[3]; 105 | qbcx = quat_bc[0]; 106 | qbcy = quat_bc[1]; 107 | qbcz = quat_bc[2]; 108 | qbcw = quat_bc[3]; 109 | /* Quaternion part is simple composition */ 110 | quat_ac[0] = qabw*qbcx + qabx*qbcw + qaby*qbcz - qabz*qbcy; 111 | quat_ac[1] = qabw*qbcy - qabx*qbcz + qaby*qbcw + qabz*qbcx; 112 | quat_ac[2] = qabw*qbcz + qabx*qbcy - qaby*qbcx + qabz*qbcw; 113 | quat_ac[3] = qabw*qbcw - qabx*qbcx - qaby*qbcy - qabz*qbcz; 114 | return 0; 115 | } 116 | 117 | int cd_kin_pose_compose(const double pose_ab[7], const double pose_bc[7], double pose_ac[7]) 118 | { 119 | double qabx, qaby, qabz, qabw; 120 | double qbcx, qbcy, qbcz, qbcw; 121 | double x_in, y_in, z_in; 122 | double x_out, y_out, z_out; 123 | double qx2, qy2, qz2, qw2, qxqy, qxqz, qxqw, qyqz, qyqw, qzqw; 124 | qabx = pose_ab[3]; 125 | qaby = pose_ab[4]; 126 | qabz = pose_ab[5]; 127 | qabw = pose_ab[6]; 128 | qbcx = pose_bc[3]; 129 | qbcy = pose_bc[4]; 130 | qbcz = pose_bc[5]; 131 | qbcw = pose_bc[6]; 132 | /* Quaternion part is simple composition */ 133 | pose_ac[3] = qabw*qbcx + qabx*qbcw + qaby*qbcz - qabz*qbcy; 134 | pose_ac[4] = qabw*qbcy - qabx*qbcz + qaby*qbcw + qabz*qbcx; 135 | pose_ac[5] = qabw*qbcz + qabx*qbcy - qaby*qbcx + qabz*qbcw; 136 | pose_ac[6] = qabw*qbcw - qabx*qbcx - qaby*qbcy - qabz*qbcz; 137 | /* Rotate xyz_bc into a frame, rotate by qab */ 138 | x_in = pose_bc[0]; 139 | y_in = pose_bc[1]; 140 | z_in = pose_bc[2]; 141 | qx2 = qabx*qabx; 142 | qy2 = qaby*qaby; 143 | qz2 = qabz*qabz; 144 | qw2 = qabw*qabw; 145 | qxqy = qabx*qaby; 146 | qxqz = qabx*qabz; 147 | qxqw = qabx*qabw; 148 | qyqz = qaby*qabz; 149 | qyqw = qaby*qabw; 150 | qzqw = qabz*qabw; 151 | x_out = x_in*(qx2-qy2-qz2+qw2) + 2*y_in*(qxqy-qzqw) + 2*z_in*(qxqz+qyqw); 152 | y_out = 2*x_in*(qxqy+qzqw) + y_in*(-qx2+qy2-qz2+qw2) + 2*z_in*(qyqz-qxqw); 153 | z_out = 2*x_in*(qxqz-qyqw) + 2*y_in*(qyqz+qxqw) + z_in*(-qx2-qy2+qz2+qw2); 154 | /* xyz part is the above rotated xyc_bc plus xyz_ab */ 155 | pose_ac[0] = x_out + pose_ab[0]; 156 | pose_ac[1] = y_out + pose_ab[1]; 157 | pose_ac[2] = z_out + pose_ab[2]; 158 | return 0; 159 | } 160 | 161 | int cd_kin_pose_compos(const double pose_ab[7], const double pos_bc[3], double pos_ac[3]) 162 | { 163 | double qabx, qaby, qabz, qabw; 164 | double x_in, y_in, z_in; 165 | double x_out, y_out, z_out; 166 | double qx2, qy2, qz2, qw2, qxqy, qxqz, qxqw, qyqz, qyqw, qzqw; 167 | qabx = pose_ab[3]; 168 | qaby = pose_ab[4]; 169 | qabz = pose_ab[5]; 170 | qabw = pose_ab[6]; 171 | /* Rotate xyz_bc into a frame, rotate by qab */ 172 | x_in = pos_bc[0]; 173 | y_in = pos_bc[1]; 174 | z_in = pos_bc[2]; 175 | qx2 = qabx*qabx; 176 | qy2 = qaby*qaby; 177 | qz2 = qabz*qabz; 178 | qw2 = qabw*qabw; 179 | qxqy = qabx*qaby; 180 | qxqz = qabx*qabz; 181 | qxqw = qabx*qabw; 182 | qyqz = qaby*qabz; 183 | qyqw = qaby*qabw; 184 | qzqw = qabz*qabw; 185 | x_out = x_in*(qx2-qy2-qz2+qw2) + 2*y_in*(qxqy-qzqw) + 2*z_in*(qxqz+qyqw); 186 | y_out = 2*x_in*(qxqy+qzqw) + y_in*(-qx2+qy2-qz2+qw2) + 2*z_in*(qyqz-qxqw); 187 | z_out = 2*x_in*(qxqz-qyqw) + 2*y_in*(qyqz+qxqw) + z_in*(-qx2-qy2+qz2+qw2); 188 | /* xyz part is the above rotated xyc_bc plus xyz_ab */ 189 | pos_ac[0] = x_out + pose_ab[0]; 190 | pos_ac[1] = y_out + pose_ab[1]; 191 | pos_ac[2] = z_out + pose_ab[2]; 192 | return 0; 193 | } 194 | 195 | int cd_kin_quat_compose_vec(const double quat_ab[4], const double vec_bc[3], double vec_ac[3]) 196 | { 197 | double qabx, qaby, qabz, qabw; 198 | double x_in, y_in, z_in; 199 | double qx2, qy2, qz2, qw2, qxqy, qxqz, qxqw, qyqz, qyqw, qzqw; 200 | qabx = quat_ab[0]; 201 | qaby = quat_ab[1]; 202 | qabz = quat_ab[2]; 203 | qabw = quat_ab[3]; 204 | /* Rotate xyz_bc into a frame, rotate by qab */ 205 | x_in = vec_bc[0]; 206 | y_in = vec_bc[1]; 207 | z_in = vec_bc[2]; 208 | qx2 = qabx*qabx; 209 | qy2 = qaby*qaby; 210 | qz2 = qabz*qabz; 211 | qw2 = qabw*qabw; 212 | qxqy = qabx*qaby; 213 | qxqz = qabx*qabz; 214 | qxqw = qabx*qabw; 215 | qyqz = qaby*qabz; 216 | qyqw = qaby*qabw; 217 | qzqw = qabz*qabw; 218 | vec_ac[0] = x_in*(qx2-qy2-qz2+qw2) + 2*y_in*(qxqy-qzqw) + 2*z_in*(qxqz+qyqw); 219 | vec_ac[1] = 2*x_in*(qxqy+qzqw) + y_in*(-qx2+qy2-qz2+qw2) + 2*z_in*(qyqz-qxqw); 220 | vec_ac[2] = 2*x_in*(qxqz-qyqw) + 2*y_in*(qyqz+qxqw) + z_in*(-qx2-qy2+qz2+qw2); 221 | return 0; 222 | } 223 | 224 | /* Do the same, but just rotate a 3d vector (vel,acc), not a position vector */ 225 | int cd_kin_pose_compose_vec(const double pose_ab[7], const double vec_bc[3], double vec_ac[3]) 226 | { 227 | double qabx, qaby, qabz, qabw; 228 | double x_in, y_in, z_in; 229 | double qx2, qy2, qz2, qw2, qxqy, qxqz, qxqw, qyqz, qyqw, qzqw; 230 | qabx = pose_ab[3]; 231 | qaby = pose_ab[4]; 232 | qabz = pose_ab[5]; 233 | qabw = pose_ab[6]; 234 | /* Rotate xyz_bc into a frame, rotate by qab */ 235 | x_in = vec_bc[0]; 236 | y_in = vec_bc[1]; 237 | z_in = vec_bc[2]; 238 | qx2 = qabx*qabx; 239 | qy2 = qaby*qaby; 240 | qz2 = qabz*qabz; 241 | qw2 = qabw*qabw; 242 | qxqy = qabx*qaby; 243 | qxqz = qabx*qabz; 244 | qxqw = qabx*qabw; 245 | qyqz = qaby*qabz; 246 | qyqw = qaby*qabw; 247 | qzqw = qabz*qabw; 248 | vec_ac[0] = x_in*(qx2-qy2-qz2+qw2) + 2*y_in*(qxqy-qzqw) + 2*z_in*(qxqz+qyqw); 249 | vec_ac[1] = 2*x_in*(qxqy+qzqw) + y_in*(-qx2+qy2-qz2+qw2) + 2*z_in*(qyqz-qxqw); 250 | vec_ac[2] = 2*x_in*(qxqz-qyqw) + 2*y_in*(qyqz+qxqw) + z_in*(-qx2-qy2+qz2+qw2); 251 | return 0; 252 | } 253 | 254 | int cd_kin_quat_invert(const double quat_in[4], double quat_out[4]) 255 | { 256 | double qx, qy, qz, qw; 257 | /* Invert quaternion (assume normalized) */ 258 | qx = -quat_in[0]; 259 | qy = -quat_in[1]; 260 | qz = -quat_in[2]; 261 | qw = quat_in[3]; 262 | quat_out[0] = qx; 263 | quat_out[1] = qy; 264 | quat_out[2] = qz; 265 | quat_out[3] = qw; 266 | return 0; 267 | } 268 | 269 | int cd_kin_pose_invert(const double pose_in[7], double pose_out[7]) 270 | { 271 | double x_in, y_in, z_in; 272 | double x_out, y_out, z_out; 273 | double qx, qy, qz, qw; 274 | double qx2, qy2, qz2, qw2, qxqy, qxqz, qxqw, qyqz, qyqw, qzqw; 275 | /* Get from input */ 276 | x_in = pose_in[0]; 277 | y_in = pose_in[1]; 278 | z_in = pose_in[2]; 279 | /* Invert quaternion (assume normalized) */ 280 | qx = -pose_in[3]; 281 | qy = -pose_in[4]; 282 | qz = -pose_in[5]; 283 | qw = pose_in[6]; 284 | /* Apply inverted quaternion to xyz_in */ 285 | qx2 = qx*qx; 286 | qy2 = qy*qy; 287 | qz2 = qz*qz; 288 | qw2 = qw*qw; 289 | qxqy = qx*qy; 290 | qxqz = qx*qz; 291 | qxqw = qx*qw; 292 | qyqz = qy*qz; 293 | qyqw = qy*qw; 294 | qzqw = qz*qw; 295 | x_out = x_in*(qx2-qy2-qz2+qw2) + 2*y_in*(qxqy-qzqw) + 2*z_in*(qxqz+qyqw); 296 | y_out = 2*x_in*(qxqy+qzqw) + y_in*(-qx2+qy2-qz2+qw2) + 2*z_in*(qyqz-qxqw); 297 | z_out = 2*x_in*(qxqz-qyqw) + 2*y_in*(qyqz+qxqw) + z_in*(-qx2-qy2+qz2+qw2); 298 | /* Apply negative x_out to output */ 299 | pose_out[0] = -x_out; 300 | pose_out[1] = -y_out; 301 | pose_out[2] = -z_out; 302 | pose_out[3] = qx; 303 | pose_out[4] = qy; 304 | pose_out[5] = qz; 305 | pose_out[6] = qw; 306 | return 0; 307 | } 308 | 309 | /* Taken from http:/www.j3d.org/matrix_faq/matrfaq_latest.html#Q54 310 | * Author: matrix_faq@j3d.org, hexapod@(no-spam)netcom.com 311 | * Formerly ftp://ftp.netcom.com/pub/he/hexapod/index.html, 312 | * http://www.glue.umd.edu/~rsrodger; 313 | * Checked (and modified) against 314 | * http://en.wikipedia.org/wiki/Rotation_representation_(mathematics) 315 | * 316 | * Further Note, from http://en.wikipedia.org/wiki/Rotation_matrix: 317 | * Efficient computation: 318 | * 319 | * Nq = w^2 + x^2 + y^2 + z^2 320 | * if Nq > 0.0 then s = 2/Nq else s = 0.0 321 | * X = x*s; Y = y*s; Z = z*s 322 | * wX = w*X; wY = w*Y; wZ = w*Z 323 | * xX = x*X; xY = x*Y; xZ = x*Z 324 | * yY = y*Y; yZ = y*Z; zZ = z*Z 325 | * [ 1.0-(yY+zZ) xY-wZ xZ+wY ] 326 | * [ xY+wZ 1.0-(xX+zZ) yZ-wX ] 327 | * [ xZ-wY yZ+wX 1.0-(xX+yY) ] 328 | */ 329 | int cd_kin_quat_to_R(const double quat[4], double R[3][3]) 330 | { 331 | double xx, xy, xz, xw, yy, yz, yw, zz, zw; 332 | xx = quat[0] * quat[0]; 333 | xy = quat[0] * quat[1]; 334 | xz = quat[0] * quat[2]; 335 | xw = quat[0] * quat[3]; 336 | yy = quat[1] * quat[1]; 337 | yz = quat[1] * quat[2]; 338 | yw = quat[1] * quat[3]; 339 | zz = quat[2] * quat[2]; 340 | zw = quat[2] * quat[3]; 341 | R[0][0] = 1 - 2 * (yy + zz); 342 | R[0][1] = 2 * (xy - zw); 343 | R[0][2] = 2 * (xz + yw); 344 | R[1][0] = 2 * (xy + zw); 345 | R[1][1] = 1 - 2 * (xx + zz); 346 | R[1][2] = 2 * (yz - xw); 347 | R[2][0] = 2 * (xz - yw); 348 | R[2][1] = 2 * (yz + xw); 349 | R[2][2] = 1 - 2 * (xx + yy); 350 | return 0; 351 | } 352 | 353 | int cd_kin_pose_to_H(const double pose[7], double H[4][4], int fill_bottom) 354 | { 355 | double xx, xy, xz, xw, yy, yz, yw, zz, zw; 356 | xx = pose[3] * pose[3]; 357 | xy = pose[3] * pose[4]; 358 | xz = pose[3] * pose[5]; 359 | xw = pose[3] * pose[6]; 360 | yy = pose[4] * pose[4]; 361 | yz = pose[4] * pose[5]; 362 | yw = pose[4] * pose[6]; 363 | zz = pose[5] * pose[5]; 364 | zw = pose[5] * pose[6]; 365 | H[0][0] = 1 - 2 * (yy + zz); 366 | H[0][1] = 2 * (xy - zw); 367 | H[0][2] = 2 * (xz + yw); 368 | H[1][0] = 2 * (xy + zw); 369 | H[1][1] = 1 - 2 * (xx + zz); 370 | H[1][2] = 2 * (yz - xw); 371 | H[2][0] = 2 * (xz - yw); 372 | H[2][1] = 2 * (yz + xw); 373 | H[2][2] = 1 - 2 * (xx + yy); 374 | H[0][3] = pose[0]; 375 | H[1][3] = pose[1]; 376 | H[2][3] = pose[2]; 377 | if (fill_bottom) 378 | { 379 | H[3][0] = 0.0; 380 | H[3][1] = 0.0; 381 | H[3][2] = 0.0; 382 | H[3][3] = 1.0; 383 | } 384 | return 0; 385 | } 386 | 387 | int cd_kin_pose_to_dR(const double pose[7], double d[3], double R[3][3]) 388 | { 389 | cd_kin_quat_to_R(pose+3, R); 390 | d[0] = pose[0]; 391 | d[1] = pose[1]; 392 | d[2] = pose[2]; 393 | return 0; 394 | } 395 | 396 | /* note that we can't easily use const double R[3][3] here, 397 | * due to 398 | */ 399 | int cd_kin_quat_from_R(double quat[4], double R[3][3]) 400 | { 401 | double xx4, yy4, zz4, ww4; /* 4 times each component */ 402 | double v4; /* 4/v with v the biggest of x, y, z, w */ 403 | xx4 = 1.0 + R[0][0] - R[1][1] - R[2][2]; 404 | yy4 = 1.0 - R[0][0] + R[1][1] - R[2][2]; 405 | zz4 = 1.0 - R[0][0] - R[1][1] + R[2][2]; 406 | ww4 = 1.0 + R[0][0] + R[1][1] + R[2][2]; 407 | if (xx4 > yy4 && xx4 > zz4 && xx4 > ww4) 408 | { 409 | quat[0] = sqrt(0.25*xx4); 410 | v4 = 0.25 / quat[0]; 411 | quat[1] = v4 * (R[1][0] + R[0][1]); 412 | quat[2] = v4 * (R[0][2] + R[2][0]); 413 | quat[3] = v4 * (R[2][1] - R[1][2]); 414 | } 415 | else if (yy4 > zz4 && yy4 > ww4) 416 | { 417 | quat[1] = sqrt(0.25*yy4); 418 | v4 = 0.25 / quat[1]; 419 | quat[0] = v4 * (R[1][0] + R[0][1]); 420 | quat[2] = v4 * (R[2][1] + R[1][2]); 421 | quat[3] = v4 * (R[0][2] - R[2][0]); 422 | } 423 | else if (zz4 > ww4) 424 | { 425 | quat[2] = sqrt(0.25*zz4); 426 | v4 = 0.25 / quat[2]; 427 | quat[0] = v4 * (R[0][2] + R[2][0]); 428 | quat[1] = v4 * (R[2][1] + R[1][2]); 429 | quat[3] = v4 * (R[1][0] - R[0][1]); 430 | } 431 | else 432 | { 433 | quat[3] = sqrt(0.25*ww4); 434 | v4 = 0.25 / quat[3]; 435 | quat[0] = v4 * (R[2][1] - R[1][2]); 436 | quat[1] = v4 * (R[0][2] - R[2][0]); 437 | quat[2] = v4 * (R[1][0] - R[0][1]); 438 | } 439 | return 0; 440 | } 441 | 442 | int cd_kin_pose_from_H(double pose[7], double H[3][4]) 443 | { 444 | double t, r, s; 445 | t = 1.0 + H[0][0] + H[1][1] + H[2][2]; 446 | if (t > 0.000001) 447 | { 448 | r = sqrt(t); 449 | s = 0.5 / r; 450 | pose[3] = (H[2][1] - H[1][2]) * s; 451 | pose[4] = (H[0][2] - H[2][0]) * s; 452 | pose[5] = (H[1][0] - H[0][1]) * s; 453 | pose[6] = 0.5 * r; 454 | } 455 | else if (H[0][0] > H[1][1] && H[0][0] > H[2][2]) 456 | { 457 | /* Rxx largest */ 458 | r = sqrt(1.0 + H[0][0] - H[1][1] - H[2][2]); 459 | s = 0.5 / r; 460 | pose[3] = 0.5 * r; 461 | pose[4] = (H[0][1] + H[1][0]) * s; 462 | pose[5] = (H[0][2] + H[2][0]) * s; 463 | pose[6] = (H[2][1] - H[1][2]) * s; 464 | } 465 | else if (H[1][1] > H[2][2]) 466 | { 467 | /* Ryy largest */ 468 | r = sqrt(1.0 - H[0][0] + H[1][1] - H[2][2]); 469 | s = 0.5 / r; 470 | pose[3] = (H[1][0] + H[0][1]) * s; 471 | pose[4] = 0.5 * r; 472 | pose[5] = (H[1][2] + H[2][1]) * s; 473 | pose[6] = (H[0][2] - H[2][0]) * s; 474 | } 475 | else 476 | { 477 | /* Rzz largest */ 478 | r = sqrt(1.0 - H[0][0] - H[1][1] + H[2][2]); 479 | s = 0.5 / r; 480 | pose[3] = (H[2][0] + H[0][2]) * s; 481 | pose[4] = (H[2][1] + H[1][2]) * s; 482 | pose[5] = 0.5 * r; 483 | pose[6] = (H[1][0] - H[0][1]) * s; 484 | } 485 | pose[0] = H[0][3]; 486 | pose[1] = H[1][3]; 487 | pose[2] = H[2][3]; 488 | return 0; 489 | } 490 | 491 | int cd_kin_pose_from_dR(double pose[7], const double d[3], double R[3][3]) 492 | { 493 | cd_kin_quat_from_R(pose+3, R); 494 | pose[0] = d[0]; 495 | pose[1] = d[1]; 496 | pose[2] = d[2]; 497 | return 0; 498 | } 499 | 500 | int cd_kin_quat_to_axisangle(const double quat[4], double axis[3], double *angle) 501 | { 502 | double a2; 503 | double sina2inv; 504 | a2 = acos(quat[3]); 505 | *angle = 2.0*a2; 506 | sina2inv = 1.0/sin(a2); 507 | axis[0] = sina2inv * quat[0]; 508 | axis[1] = sina2inv * quat[1]; 509 | axis[2] = sina2inv * quat[2]; 510 | return 0; 511 | } 512 | 513 | int cd_kin_quat_from_axisangle(double quat[4], const double axis[3], const double angle) 514 | { 515 | double a2; 516 | double sina2; 517 | a2 = 0.5*angle; 518 | quat[3] = cos(a2); 519 | sina2 = sin(a2); 520 | quat[0] = sina2 * axis[0]; 521 | quat[1] = sina2 * axis[1]; 522 | quat[2] = sina2 * axis[2]; 523 | return 0; 524 | } 525 | 526 | /* using Rodrigues' rotation formula 527 | * http://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula */ 528 | int cd_kin_axisangle_rotate(const double axis[3], const double angle, 529 | const double pos_in[3], double pos_out[3]) 530 | { 531 | double v[3]; 532 | double axisdotv; 533 | cd_mat_memcpy(v, pos_in, 3, 1); 534 | cd_mat_set_zero(pos_out, 3, 1); 535 | cd_mat_cross(axis, v, pos_out); 536 | cd_mat_scale(pos_out, 3, 1, sin(angle)); 537 | cblas_daxpy(3, cos(angle), v,1, pos_out,1); 538 | axisdotv = cblas_ddot(3, axis,1, v,1); 539 | cblas_daxpy(3, axisdotv*(1-cos(angle)), axis,1, pos_out,1); 540 | return 0; 541 | } 542 | 543 | /* exponential map, so(3) -> SO(3) */ 544 | int cd_kin_axisangle_to_R(const double axis[3], const double angle, double R[3][3]) 545 | { 546 | double x, y, z; 547 | double s; 548 | double cm; 549 | s = sin(angle); 550 | cm = 1.0 - cos(angle); 551 | x = axis[0]; 552 | y = axis[1]; 553 | z = axis[2]; 554 | R[0][0] = 1.0 - cm*(y*y + z*z); 555 | R[0][1] = -s*z + cm*x*y; 556 | R[0][2] = s*y + cm*x*z; 557 | R[1][0] = s*z + cm*x*y; 558 | R[1][1] = 1.0 - cm*(x*x + z*z); 559 | R[1][2] = -s*x + cm*y*z; 560 | R[2][0] = -s*y + cm*x*z; 561 | R[2][1] = s*x + cm*y*z; 562 | R[2][2] = 1.0 - cm*(x*x + y*y); 563 | return 0; 564 | } 565 | 566 | int cd_kin_quat_to_ypr(const double quat[4], double ypr[3]) 567 | { 568 | double qx, qy, qz, qw; 569 | double sinp2; 570 | qx = quat[0]; 571 | qy = quat[1]; 572 | qz = quat[2]; 573 | qw = quat[3]; 574 | sinp2 = qw*qy-qz*qx; 575 | if (sinp2 > 0.49999) 576 | { 577 | ypr[0] = -2.0*atan2(qx,qw); 578 | ypr[1] = 0.25*TAU; 579 | ypr[2] = 0.0; 580 | } 581 | else if (sinp2 < -0.49999) 582 | { 583 | ypr[0] = 2.0*atan2(qx,qw); 584 | ypr[1] = -0.25*TAU; 585 | ypr[2] = 0.0; 586 | } 587 | else 588 | { 589 | ypr[0] = atan2(2.0*(qw*qz+qx*qy), 1.0 - 2.0*(qy*qy+qz*qz)); 590 | ypr[1] = asin(2.0*sinp2); 591 | ypr[2] = atan2(2.0*(qw*qx+qy*qz), 1.0 - 2.0*(qx*qx+qy*qy)); 592 | } 593 | return 0; 594 | } 595 | 596 | int cd_kin_pose_to_xyzypr(const double pose[7], double xyzypr[6]) 597 | { 598 | double qx, qy, qz, qw; 599 | double sinp2; 600 | qx = pose[3]; 601 | qy = pose[4]; 602 | qz = pose[5]; 603 | qw = pose[6]; 604 | xyzypr[0] = pose[0]; 605 | xyzypr[1] = pose[1]; 606 | xyzypr[2] = pose[2]; 607 | sinp2 = qw*qy-qz*qx; 608 | if (sinp2 > 0.49999) 609 | { 610 | xyzypr[3] = -2.0*atan2(qx,qw); 611 | xyzypr[4] = 0.25*TAU; 612 | xyzypr[5] = 0.0; 613 | } 614 | else if (sinp2 < -0.49999) 615 | { 616 | xyzypr[3] = 2.0*atan2(qx,qw); 617 | xyzypr[4] = -0.25*TAU; 618 | xyzypr[5] = 0.0; 619 | } 620 | else 621 | { 622 | xyzypr[3] = atan2(2.0*(qw*qz+qx*qy), 1.0 - 2.0*(qy*qy+qz*qz)); 623 | xyzypr[4] = asin(2.0*sinp2); 624 | xyzypr[5] = atan2(2.0*(qw*qx+qy*qz), 1.0 - 2.0*(qx*qx+qy*qy)); 625 | } 626 | return 0; 627 | } 628 | 629 | /* for now, dont deal with gimbal lock! */ 630 | int cd_kin_quat_to_ypr_J(const double quat[4], double J[3][4]) 631 | { 632 | double qx, qy, qz, qw; 633 | double nu, de; /* numerator, denominator */ 634 | double as; /* argsin arg */ 635 | qx = quat[0]; 636 | qy = quat[1]; 637 | qz = quat[2]; 638 | qw = quat[3]; 639 | /* yaw */ 640 | nu = 2.0*(qw*qz+qx*qy); 641 | de = 1.0 - 2.0*(qy*qy+qz*qz); 642 | J[0][0] = de/(de*de+nu*nu)*(2.0*qy); 643 | J[0][1] = de/(de*de+nu*nu)*(2.0*qx) - nu/(de*de+nu*nu)*(-2.0*2.0*qy); 644 | J[0][2] = de/(de*de+nu*nu)*(2.0*qw) - nu/(de*de+nu*nu)*(-2.0*2.0*qz); 645 | J[0][3] = de/(de*de+nu*nu)*(2.0*qz); 646 | /* pitch */ 647 | as = 2.0 * (qw*qy-qz*qx); 648 | J[1][0] = 1.0/sqrt(1.0-as*as)*2.0*(-qz); 649 | J[1][1] = 1.0/sqrt(1.0-as*as)*2.0*( qw); 650 | J[1][2] = 1.0/sqrt(1.0-as*as)*2.0*(-qx); 651 | J[1][3] = 1.0/sqrt(1.0-as*as)*2.0*( qy); 652 | /* roll */ 653 | nu = 2.0*(qw*qx+qy*qz); 654 | de = 1.0 - 2.0*(qx*qx+qy*qy); 655 | J[2][0] = de/(de*de+nu*nu)*(2.0*qw) - nu/(de*de+nu*nu)*(-2.0*2.0*qx); 656 | J[2][1] = de/(de*de+nu*nu)*(2.0*qz) - nu/(de*de+nu*nu)*(-2.0*2.0*qy); 657 | J[2][2] = de/(de*de+nu*nu)*(2.0*qy); 658 | J[2][3] = de/(de*de+nu*nu)*(2.0*qx); 659 | return 0; 660 | } 661 | 662 | /* for now, dont deal with gimbal lock! */ 663 | int cd_kin_pose_to_xyzypr_J(const double pose[7], double J[6][7]) 664 | { 665 | double qx, qy, qz, qw; 666 | double nu, de; /* numerator, denominator */ 667 | double as; /* argsin arg */ 668 | qx = pose[3]; 669 | qy = pose[4]; 670 | qz = pose[5]; 671 | qw = pose[6]; 672 | /* position part */ 673 | cd_mat_set_zero(*J, 6, 7); 674 | J[0][0] = 1.0; 675 | J[1][1] = 1.0; 676 | J[2][2] = 1.0; 677 | /* yaw */ 678 | nu = 2.0*(qw*qz+qx*qy); 679 | de = 1.0 - 2.0*(qy*qy+qz*qz); 680 | J[3][3] = de/(de*de+nu*nu)*(2.0*qy); 681 | J[3][4] = de/(de*de+nu*nu)*(2.0*qx) - nu/(de*de+nu*nu)*(-2.0*2.0*qy); 682 | J[3][5] = de/(de*de+nu*nu)*(2.0*qw) - nu/(de*de+nu*nu)*(-2.0*2.0*qz); 683 | J[3][6] = de/(de*de+nu*nu)*(2.0*qz); 684 | /* pitch */ 685 | as = 2.0 * (qw*qy-qz*qx); 686 | J[4][3] = 1.0/sqrt(1.0-as*as)*2.0*(-qz); 687 | J[4][4] = 1.0/sqrt(1.0-as*as)*2.0*( qw); 688 | J[4][5] = 1.0/sqrt(1.0-as*as)*2.0*(-qx); 689 | J[4][6] = 1.0/sqrt(1.0-as*as)*2.0*( qy); 690 | /* roll */ 691 | nu = 2.0*(qw*qx+qy*qz); 692 | de = 1.0 - 2.0*(qx*qx+qy*qy); 693 | J[5][3] = de/(de*de+nu*nu)*(2.0*qw) - nu/(de*de+nu*nu)*(-2.0*2.0*qx); 694 | J[5][4] = de/(de*de+nu*nu)*(2.0*qz) - nu/(de*de+nu*nu)*(-2.0*2.0*qy); 695 | J[5][5] = de/(de*de+nu*nu)*(2.0*qy); 696 | J[5][6] = de/(de*de+nu*nu)*(2.0*qx); 697 | return 0; 698 | } 699 | 700 | int cd_kin_quat_from_ypr(double quat[4], const double ypr[3]) 701 | { 702 | double cy2, sy2, cp2, sp2, cr2, sr2; 703 | cy2 = cos(0.5 * ypr[0]); 704 | sy2 = sin(0.5 * ypr[0]); 705 | cp2 = cos(0.5 * ypr[1]); 706 | sp2 = sin(0.5 * ypr[1]); 707 | cr2 = cos(0.5 * ypr[2]); 708 | sr2 = sin(0.5 * ypr[2]); 709 | quat[0] = -sy2*sp2*cr2 + cy2*cp2*sr2; /* qx */ 710 | quat[1] = cy2*sp2*cr2 + sy2*cp2*sr2; /* qy */ 711 | quat[2] = -cy2*sp2*sr2 + sy2*cp2*cr2; /* qz */ 712 | quat[3] = sy2*sp2*sr2 + cy2*cp2*cr2; /* qw */ 713 | return 0; 714 | } 715 | 716 | int cd_kin_pose_from_xyzypr(double pose[7], const double xyzypr[6]) 717 | { 718 | double cy2, sy2, cp2, sp2, cr2, sr2; 719 | cy2 = cos(0.5 * xyzypr[3]); 720 | sy2 = sin(0.5 * xyzypr[3]); 721 | cp2 = cos(0.5 * xyzypr[4]); 722 | sp2 = sin(0.5 * xyzypr[4]); 723 | cr2 = cos(0.5 * xyzypr[5]); 724 | sr2 = sin(0.5 * xyzypr[5]); 725 | pose[0] = xyzypr[0]; 726 | pose[1] = xyzypr[1]; 727 | pose[2] = xyzypr[2]; 728 | pose[3] = -sy2*sp2*cr2 + cy2*cp2*sr2; /* qx */ 729 | pose[4] = cy2*sp2*cr2 + sy2*cp2*sr2; /* qy */ 730 | pose[5] = -cy2*sp2*sr2 + sy2*cp2*cr2; /* qz */ 731 | pose[6] = sy2*sp2*sr2 + cy2*cp2*cr2; /* qw */ 732 | return 0; 733 | } 734 | 735 | int cd_kin_pose_to_pos_quat(const double pose[7], double pos[3], double quat[4]) 736 | { 737 | int i; 738 | if (pos) for (i=0; i<3; i++) pos[i] = pose[i]; 739 | if (quat) for (i=0; i<4; i++) quat[i] = pose[3+i]; 740 | return 0; 741 | } 742 | 743 | int cd_kin_pose_from_pos_quat(double pose[7], const double pos[3], const double quat[4]) 744 | { 745 | int i; 746 | if (pos) for (i=0; i<3; i++) pose[i] = pos[i]; 747 | else for (i=0; i<3; i++) pose[i] = 0.0; 748 | if (quat) for (i=0; i<4; i++) pose[3+i] = quat[i]; 749 | else for (i=0; i<4; i++) pose[3+i] = i==3 ? 1.0 : 0.0; 750 | return 0; 751 | } 752 | 753 | int cd_kin_pose_from_op(double pose[7], const double from[3], const double to[3], double * const lenp) 754 | { 755 | double to_diff[3]; 756 | to_diff[0] = to[0]; 757 | to_diff[1] = to[1]; 758 | to_diff[2] = to[2]; 759 | if (from) 760 | { 761 | to_diff[0] -= from[0]; 762 | to_diff[1] -= from[1]; 763 | to_diff[2] -= from[2]; 764 | } 765 | cd_kin_pose_from_op_diff(pose, from, to_diff, lenp); 766 | return 0; 767 | } 768 | 769 | int cd_kin_pose_from_op_diff(double pose[7], const double from[3], const double to_diff[3], double * const lenp) 770 | { 771 | double d[3]; 772 | double R[3][3]; 773 | double len; 774 | 775 | if (from) 776 | { 777 | d[0] = from[0]; 778 | d[1] = from[1]; 779 | d[2] = from[2]; 780 | } 781 | else 782 | { 783 | d[0] = 0.0; 784 | d[1] = 0.0; 785 | d[2] = 0.0; 786 | } 787 | 788 | /* Define Z axis in direction of arrow */ 789 | len = cblas_dnrm2(3, to_diff, 1); 790 | if (lenp) *lenp = len; 791 | R[0][2] = to_diff[0] / len; 792 | R[1][2] = to_diff[1] / len; 793 | R[2][2] = to_diff[2] / len; 794 | 795 | /* Define other axes */ 796 | if (fabs(R[0][2]) > 0.9) 797 | { 798 | /* Z is too close to e1, but sufficiently far from e2; 799 | * cross e2 with Z to get X (and normalize) */ 800 | len = sqrt(R[2][2]*R[2][2] + R[0][2]*R[0][2]); 801 | R[0][0] = R[2][2] / len; 802 | R[1][0] = 0.0; 803 | R[2][0] = -R[0][2] / len; 804 | /* Then Y = Z x X */ 805 | R[0][1] = R[1][2] * R[2][0] - R[2][2] * R[1][0]; 806 | R[1][1] = R[2][2] * R[0][0] - R[0][2] * R[2][0]; 807 | R[2][1] = R[0][2] * R[1][0] - R[1][2] * R[0][0]; 808 | } 809 | else 810 | { 811 | /* Z is sufficiently far from e1; 812 | * cross Z with e1 to get Y (and normalize) */ 813 | len = sqrt(R[2][2]*R[2][2] + R[1][2]*R[1][2]); 814 | R[0][1] = 0.0; 815 | R[1][1] = R[2][2] / len; 816 | R[2][1] = -R[1][2] / len; 817 | /* Then X = Y x Z */ 818 | R[0][0] = R[1][1] * R[2][2] - R[2][1] * R[1][2]; 819 | R[1][0] = R[2][1] * R[0][2] - R[0][1] * R[2][2]; 820 | R[2][0] = R[0][1] * R[1][2] - R[1][1] * R[0][2]; 821 | } 822 | 823 | cd_kin_pose_from_dR(pose, d, R); 824 | return 0; 825 | } --------------------------------------------------------------------------------