Unverified Commit 000ef447 authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #1013 from jcarpent/devel

Add new examples
parents c7fc0fab febbac59
...@@ -11,6 +11,9 @@ SET(${PROJECT_NAME}_EXAMPLES ...@@ -11,6 +11,9 @@ SET(${PROJECT_NAME}_EXAMPLES
interpolation-SE3 interpolation-SE3
build-reduced-model build-reduced-model
geometry-models geometry-models
kinematics-derivatives
forward-dynamics-derivatives
inverse-dynamics-derivatives
) )
IF(HPP_FCL_FOUND) IF(HPP_FCL_FOUND)
...@@ -43,8 +46,8 @@ IF(BUILD_PYTHON_INTERFACE) ...@@ -43,8 +46,8 @@ IF(BUILD_PYTHON_INTERFACE)
overview-simple overview-simple
overview-urdf overview-urdf
kinematics-derivatives kinematics-derivatives
fd-derivatives forward-dynamics-derivatives
id-derivatives inverse-dynamics-derivatives
gepetto-viewer gepetto-viewer
meshcat-viewer meshcat-viewer
meshcat-viewer-dae meshcat-viewer-dae
...@@ -56,11 +59,11 @@ IF(BUILD_PYTHON_INTERFACE) ...@@ -56,11 +59,11 @@ IF(BUILD_PYTHON_INTERFACE)
LIST(APPEND ${PROJECT_NAME}_PYTHON_EXAMPLES LIST(APPEND ${PROJECT_NAME}_PYTHON_EXAMPLES
collisions collisions
sample-model-viewer sample-model-viewer
display_shapes display-shapes
) )
ENDIF(HPP_FCL_FOUND) ENDIF(HPP_FCL_FOUND)
FOREACH(EXAMPLE ${${PROJECT_NAME}_PYTHON_EXAMPLES}) FOREACH(EXAMPLE ${${PROJECT_NAME}_PYTHON_EXAMPLES})
ADD_PYTHON_UNIT_TEST("py-example-${EXAMPLE}" "examples/${EXAMPLE}.py" "bindings/python") ADD_PYTHON_UNIT_TEST("example-py-${EXAMPLE}" "examples/${EXAMPLE}.py" "bindings/python")
ENDFOREACH(EXAMPLE ${${PROJECT_NAME}_PYTHON_EXAMPLES}) ENDFOREACH(EXAMPLE ${${PROJECT_NAME}_PYTHON_EXAMPLES})
ENDIF(BUILD_PYTHON_INTERFACE) ENDIF(BUILD_PYTHON_INTERFACE)
# Pinocchio examples in Python # Pinocchio examples in Python
This directory contains minimal examples on how to use **Pinocchio** with the Python bindings. This directory contains minimal examples on how to use **Pinocchio** with the Python bindings or directly in C++.
## Loading a model ## Loading a model
- Loading a URDF model: `python -i overview-urdf.py` - Loading an embeded Model: `python -i overview-simple.py` and in C++ `g++ -I $(pkg-config --cflags pinocchio) -g overview-simple.cpp -o overview-simple && ./overview-simple`
- Loading a URDF model: `python -i overview-urdf.py` and in C++ `g++ -I $(pkg-config --cflags pinocchio) -g overview-urdf.cpp -o overview-urdf && ./overview-urdf`
- Using RobotWrapper to encapsulate a URDF model: `python -i robot-wrapper-viewer.py` - Using RobotWrapper to encapsulate a URDF model: `python -i robot-wrapper-viewer.py`
## Computing analytical derivatives of rigid body dynamics algorithms ## Computes analytical derivatives of rigid body dynamics algorithms
- Computing forward kinematics derivatives: `python -i kinematics-derivatives.py` - Computing forward kinematics derivatives: `python -i kinematics-derivatives.py` and in C++ `g++ -I $(pkg-config --cflags pinocchio) -g kinematics-derivatives.cpp -o kinematics-derivatives && ./kinematics-derivatives`
- Computing forward dynamics (fd) derivatives: `python -i fd-derivatives.py` - Computing forward dynamics derivatives: `python -i forward-dynamics-derivatives.py` and in C++ `g++ -I $(pkg-config --cflags pinocchio) -g forward-dynamics-derivatives.cpp -o forward-dynamics-derivatives && ./forward-dynamics-derivatives`
- Computing inverse dynamics (id) derivatives: `python -i id-derivatives.py` - Computing inverse dynamics derivatives: `python -i inverse-dynamics-derivatives.py` and in C++ `g++ -I $(pkg-config --cflags pinocchio) -g inverse-dynamics-derivatives.cpp -o inverse-dynamics-derivatives && ./inverse-derivatives`
## Viewer with Pinocchio ## Displaying the models
For the following examples, you should have [gepetto-gui](https://github.com/Gepetto/gepetto-viewer-corba) or [MeshCat](https://github.com/rdeits/meshcat) installed.
- Loading a robot model using [gepetto-gui](https://github.com/Gepetto/gepetto-viewer-corba): `python -i gepetto-viewer.py` - Loading a robot model using [gepetto-gui](https://github.com/Gepetto/gepetto-viewer-corba): `python -i gepetto-viewer.py`
- Loading a robot model using [MeshCat](https://github.com/rdeits/meshcat): `python -i meshcat-viewer.py` - Loading a robot model using [MeshCat](https://github.com/rdeits/meshcat): `python -i meshcat-viewer.py`
- Loading a model with basic geometries [MeshCat](https://github.com/rdeits/meshcat): `python -i sample-model-viewer.py`
## Collision checking
Pinocchio encapsulates [FCL](https://github.com/humanoid-path-planner/hpp-fcl) in it. You can then do collision checking or distance computations with only few lines of code.
- Check collisions using [FCL](https://github.com/humanoid-path-planner/hpp-fcl): `python -i collisions.py` and in C++ `g++ -I $(pkg-config --cflags pinocchio) -g collision.cpp -o collision && ./collision`
## Collisions ## Adding new examples
- Check collisions using [FCL](https://github.com/humanoid-path-planner/hpp-fcl): `python -i collisions.py` If you need a specific example for your applications, to not hesitate to open an issue detailing your specific request.
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/aba-derivatives.hpp"
#include <iostream>
// PINOCCHIO_MODEL_DIR is defined by the CMake but you can define your own directory here.
#ifndef PINOCCHIO_MODEL_DIR
#define PINOCCHIO_MODEL_DIR "path_to_the_model_dir"
#endif
int main(int argc, char ** argv)
{
using namespace pinocchio;
// You should change here to set up your own URDF file or just pass it as an argument of this example.
const std::string urdf_filename = (argc<=1) ? PINOCCHIO_MODEL_DIR + std::string("/others/robots/ur_description/urdf/ur5_robot.urdf") : argv[1];
// Load the URDF model
Model model;
pinocchio::urdf::buildModel(urdf_filename,model);
// Build a data related to model
Data data(model);
// Sample a random joint configuration as well as random joint velocity and acceleration
Eigen::VectorXd q = randomConfiguration(model);
Eigen::VectorXd v = Eigen::VectorXd(model.nv);
Eigen::VectorXd tau = Eigen::VectorXd(model.nv);
// Allocate result container
Eigen::MatrixXd djoint_acc_dq = Eigen::MatrixXd::Zero(model.nv,model.nv);
Eigen::MatrixXd djoint_acc_dv = Eigen::MatrixXd::Zero(model.nv,model.nv);
Eigen::MatrixXd djoint_acc_dtau = Eigen::MatrixXd::Zero(model.nv,model.nv);
// Computes the forward dynamics (ABA) derivatives for all the joints of the robot
computeABADerivatives(model, data, q, v, tau, djoint_acc_dq, djoint_acc_dv, djoint_acc_dtau);
// Get access to the joint acceleration
std::cout << "Joint acceleration: " << data.ddq.transpose() << std::endl;
}
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/rnea-derivatives.hpp"
#include <iostream>
// PINOCCHIO_MODEL_DIR is defined by the CMake but you can define your own directory here.
#ifndef PINOCCHIO_MODEL_DIR
#define PINOCCHIO_MODEL_DIR "path_to_the_model_dir"
#endif
int main(int argc, char ** argv)
{
using namespace pinocchio;
// You should change here to set up your own URDF file or just pass it as an argument of this example.
const std::string urdf_filename = (argc<=1) ? PINOCCHIO_MODEL_DIR + std::string("/others/robots/ur_description/urdf/ur5_robot.urdf") : argv[1];
// Load the URDF model
Model model;
pinocchio::urdf::buildModel(urdf_filename,model);
// Build a data related to model
Data data(model);
// Sample a random joint configuration as well as random joint velocity and acceleration
Eigen::VectorXd q = randomConfiguration(model);
Eigen::VectorXd v = Eigen::VectorXd(model.nv);
Eigen::VectorXd a = Eigen::VectorXd(model.nv);
// Allocate result container
Eigen::MatrixXd djoint_torque_dq = Eigen::MatrixXd::Zero(model.nv,model.nv);
Eigen::MatrixXd djoint_torque_dv = Eigen::MatrixXd::Zero(model.nv,model.nv);
Eigen::MatrixXd djoint_torque_da = Eigen::MatrixXd::Zero(model.nv,model.nv);
// Computes the inverse dynamics (RNEA) derivatives for all the joints of the robot
computeRNEADerivatives(model, data, q, v, a, djoint_torque_dq, djoint_torque_dv, djoint_torque_da);
// Get access to the joint torque
std::cout << "Joint torque: " << data.tau.transpose() << std::endl;
}
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/kinematics-derivatives.hpp"
#include <iostream>
// PINOCCHIO_MODEL_DIR is defined by the CMake but you can define your own directory here.
#ifndef PINOCCHIO_MODEL_DIR
#define PINOCCHIO_MODEL_DIR "path_to_the_model_dir"
#endif
int main(int argc, char ** argv)
{
using namespace pinocchio;
// You should change here to set up your own URDF file or just pass it as an argument of this example.
const std::string urdf_filename = (argc<=1) ? PINOCCHIO_MODEL_DIR + std::string("/others/robots/ur_description/urdf/ur5_robot.urdf") : argv[1];
// Load the URDF model
Model model;
pinocchio::urdf::buildModel(urdf_filename,model);
// Build a data related to model
Data data(model);
// Sample a random joint configuration as well as random joint velocity and acceleration
Eigen::VectorXd q = randomConfiguration(model);
Eigen::VectorXd v = Eigen::VectorXd(model.nv);
Eigen::VectorXd a = Eigen::VectorXd(model.nv);
// Computes the kinematics derivatives for all the joints of the robot
computeForwardKinematicsDerivatives(model, data, q, v, a);
// Retrieve the kinematics derivatives of a specific joint, expressed in the LOCAL frame of the joints.
JointIndex joint_id = (JointIndex)(model.njoints-1);
Data::Matrix6x v_partial_dq(6,model.nv), a_partial_dq(6,model.nv), a_partial_dv(6,model.nv), a_partial_da(6,model.nv);
v_partial_dq.setZero();
a_partial_dq.setZero(); a_partial_dv.setZero(); a_partial_da.setZero();
getJointAccelerationDerivatives(model,data,joint_id,LOCAL,v_partial_dq,
a_partial_dq,a_partial_dv,a_partial_da);
// Remark: we are not directly computing the quantity v_partial_dv as it is also equal to a_partial_da.
// But we can also expressed the same quantities in the frame centered on the end-effector joint, but expressed in the axis aligned with the world frame.
getJointAccelerationDerivatives(model,data,joint_id,WORLD,v_partial_dq,
a_partial_dq,a_partial_dv,a_partial_da);
}
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment