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

Merge pull request #1436 from jcarpent/topic/examples

Update examples + improve doc
parents c153dfd7 ef25eba6
Pipeline #14084 passed with stage
in 100 minutes and 4 seconds
......@@ -147,15 +147,15 @@ namespace pinocchio
.def("FromSphere", &Inertia::FromSphere,
bp::args("mass","radius"),
"Returns the Inertia of an sphere with a given mass and of radius.")
"Returns the Inertia of a sphere defined by a given mass and radius.")
.staticmethod("FromSphere")
.def("FromEllipsoid", &Inertia::FromEllipsoid,
bp::args("mass","length_x","length_y","length_z"),
"Returns the Inertia of an ellipsoid shape with a given mass and of given dimensions the semi-axis of values length_{x,y,z}.")
"Returns the Inertia of an ellipsoid shape defined by a mass and given dimensions the semi-axis of values length_{x,y,z}.")
.staticmethod("FromEllipsoid")
.def("FromCylinder", &Inertia::FromCylinder,
bp::args("mass","radius","length"),
"Returns the Inertia of a cylinder shape ith a mass and of dimension radius and length.")
"Returns the Inertia of a cylinder defined by its mass, radius and length along the Z axis.")
.staticmethod("FromCylinder")
.def("FromBox", &Inertia::FromBox,
bp::args("mass","length_x","length_y","length_z"),
......
......@@ -83,7 +83,7 @@ IF(BUILD_PYTHON_INTERFACE)
LIST(APPEND ${PROJECT_NAME}_PYTHON_EXAMPLES
sample-model-viewer
display-shapes
simulation-inverted-pendulum
simulation-pendulum
)
IF(BUILD_WITH_URDF_SUPPORT)
LIST(APPEND ${PROJECT_NAME}_PYTHON_EXAMPLES
......
......@@ -24,6 +24,10 @@ pip install panda3d_viewer
- Loading a URDF model: `python -i overview-urdf.py` and in C++ `g++ -g overview-urdf.cpp -o overview-urdf $(pkg-config --cflags --libs pinocchio) && ./overview-urdf`
- Using RobotWrapper to encapsulate a URDF model: `python -i robot-wrapper-viewer.py`
## Simulating a model
- Simulating a multiple pendulum: `python -i simulation-pendulum.py --with-cart -N=2`
## Computes analytical derivatives of rigid body dynamics algorithms
- Computing forward kinematics derivatives: `python -i kinematics-derivatives.py` and in C++ `g++ kinematics-derivatives.cpp -o kinematics-derivatives $(pkg-config --cflags --libs pinocchio) && ./kinematics-derivatives`
......
......@@ -5,23 +5,61 @@ import math
import time
import sys
N = 10 # number of pendulums
# Parse input arguments
import argparse
parser = argparse.ArgumentParser()
parser.add_argument("--with-cart", help="Add a cart at the base of the pendulum to simulate a cart pole system.",
action="store_true")
parser.add_argument("-N", help="Number of pendulums compositing the dynamical system.",
type=int)
args = parser.parse_args()
if args.N:
N = args.N
else:
N = 1 # number of pendulums
model = pin.Model()
geom_model = pin.GeometryModel()
parent_id = 0
if args.with_cart:
cart_radius = 0.1
cart_length = 5 * cart_radius
cart_mass = 2.
joint_name = "joint_cart"
geometry_placement = pin.SE3.Identity()
geometry_placement.rotation = pin.Quaternion(np.array([0.,0.,1.]),np.array([0.,1.,0.])).toRotationMatrix()
joint_id = model.addJoint(parent_id, pin.JointModelPY(), pin.SE3.Identity(), joint_name)
body_inertia = pin.Inertia.FromCylinder(cart_mass,cart_radius,cart_length)
body_placement = geometry_placement
model.appendBodyToJoint(joint_id,body_inertia,body_placement) # We need to rotate the inertia as it is expressed in the LOCAL frame of the geometry
shape_cart = fcl.Cylinder(cart_radius, cart_length)
geom_cart = pin.GeometryObject("shape_cart", joint_id, shape_cart, geometry_placement)
geom_cart.meshColor = np.array([1.,0.1,0.1,1.])
geom_model.addGeometryObject(geom_cart)
parent_id = joint_id
else:
base_radius = 0.2
shape_base = fcl.Sphere(base_radius)
geom_base = pin.GeometryObject("base", 0, shape_base, pin.SE3.Identity())
geom_base.meshColor = np.array([1.,0.1,0.1,1.])
geom_model.addGeometryObject(geom_base)
joint_placement = pin.SE3.Identity()
body_mass = 1.
body_radius = 0.1
shape0 = fcl.Sphere(body_radius)
geom0_obj = pin.GeometryObject("base", 0, shape0, pin.SE3.Identity())
geom0_obj.meshColor = np.array([1.,0.1,0.1,1.])
geom_model.addGeometryObject(geom0_obj)
for k in range(N):
joint_name = "joint_" + str(k+1)
joint_id = model.addJoint(parent_id,pin.JointModelRY(),joint_placement,joint_name)
joint_id = model.addJoint(parent_id,pin.JointModelRX(),joint_placement,joint_name)
body_inertia = pin.Inertia.FromSphere(body_mass,body_radius)
body_placement = joint_placement.copy()
......@@ -46,10 +84,10 @@ for k in range(N):
parent_id = joint_id
joint_placement = body_placement.copy()
from pinocchio.visualize import GepettoVisualizer
from pinocchio.visualize import MeshcatVisualizer as Visualizer
visual_model = geom_model
viz = GepettoVisualizer(model, geom_model, visual_model)
viz = Visualizer(model, geom_model, visual_model)
# Initialize the viewer.
try:
......@@ -78,19 +116,31 @@ N = math.floor(T/dt)
model.lowerPositionLimit.fill(-math.pi)
model.upperPositionLimit.fill(+math.pi)
q = pin.randomConfiguration(model)
v = np.zeros((model.nv))
t = 0.
if args.with_cart:
model.lowerPositionLimit[0] = model.upperPositionLimit[0] = 0.
data_sim = model.createData()
t = 0.
q = pin.randomConfiguration(model)
v = np.zeros((model.nv))
tau_control = np.zeros((model.nv))
damping_value = 0.1
for k in range(N):
tau_control = np.zeros((model.nv))
tic = time.time()
tau_control = -damping_value * v # small damping
a = pin.aba(model,data_sim,q,v,tau_control) # Forward dynamics
# Semi-explicit integration
v += a*dt
#q += v*dt
q = pin.integrate(model,q,v*dt)
q = pin.integrate(model,q,v*dt) # Configuration integration
viz.display(q)
time.sleep(dt)
toc = time.time()
ellapsed = toc - tic
dt_sleep = max(0,dt - (ellapsed))
time.sleep(dt_sleep)
t += dt
//
// Copyright (c) 2015-2019 CNRS INRIA
// Copyright (c) 2015-2021 CNRS INRIA
// Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
......@@ -238,37 +238,73 @@ namespace pinocchio
Symmetric3::RandomPositive());
}
static InertiaTpl FromSphere(const Scalar m, const Scalar radius)
///
/// \brief Computes the Inertia of a sphere defined by its mass and its radius.
///
/// \param[in] mass of the sphere.
/// \param[in] radius of the sphere.
///
static InertiaTpl FromSphere(const Scalar mass, const Scalar radius)
{
return FromEllipsoid(m,radius,radius,radius);
return FromEllipsoid(mass,radius,radius,radius);
}
static InertiaTpl FromEllipsoid(const Scalar m, const Scalar x,
const Scalar y, const Scalar z)
///
/// \brief Computes the Inertia of an ellipsoid defined by its mass and main semi-axis dimensions (x,y,z).
///
/// \param[in] mass of the ellipsoid.
/// \param[in] x semi-axis dimension along the local X axis.
/// \param[in] y semi-axis dimension along the local Y axis.
/// \param[in] z semi-axis dimension along the local Z axis.
///
static InertiaTpl FromEllipsoid(const Scalar mass,
const Scalar x,
const Scalar y,
const Scalar z)
{
Scalar a = m * (y*y + z*z) / Scalar(5);
Scalar b = m * (x*x + z*z) / Scalar(5);
Scalar c = m * (y*y + x*x) / Scalar(5);
return InertiaTpl(m, Vector3::Zero(), Symmetric3(a, Scalar(0), b,
Scalar(0), Scalar(0), c));
const Scalar a = mass * (y*y + z*z) / Scalar(5);
const Scalar b = mass * (x*x + z*z) / Scalar(5);
const Scalar c = mass * (y*y + x*x) / Scalar(5);
return InertiaTpl(mass, Vector3::Zero(), Symmetric3(a, Scalar(0), b,
Scalar(0), Scalar(0), c));
}
static InertiaTpl FromCylinder(const Scalar m, const Scalar r, const Scalar l)
///
/// \brief Computes the Inertia of a cylinder defined by its mass, radius and length along the Z axis.
///
/// \param[in] mass of the cylinder.
/// \param[in] radius of the cylinder.
/// \param[in] length of the cylinder.
///
static InertiaTpl FromCylinder(const Scalar mass,
const Scalar radius,
const Scalar length)
{
Scalar a = m * (r*r / Scalar(4) + l*l / Scalar(12));
Scalar c = m * (r*r / Scalar(2));
return InertiaTpl(m, Vector3::Zero(), Symmetric3(a, Scalar(0), a,
Scalar(0), Scalar(0), c));
const Scalar radius_square = radius * radius;
const Scalar a = mass * (radius_square / Scalar(4) + length*length / Scalar(12));
const Scalar c = mass * (radius_square / Scalar(2));
return InertiaTpl(mass, Vector3::Zero(), Symmetric3(a, Scalar(0), a,
Scalar(0), Scalar(0), c));
}
static InertiaTpl FromBox(const Scalar m, const Scalar x,
const Scalar y, const Scalar z)
{
Scalar a = m * (y*y + z*z) / Scalar(12);
Scalar b = m * (x*x + z*z) / Scalar(12);
Scalar c = m * (y*y + x*x) / Scalar(12);
return InertiaTpl(m, Vector3::Zero(), Symmetric3(a, Scalar(0), b,
Scalar(0), Scalar(0), c));
///
/// \brief Computes the Inertia of a box defined by its mass and main dimensions (x,y,z).
///
/// \param[in] mass of the box.
/// \param[in] x dimension along the local X axis.
/// \param[in] y dimension along the local Y axis.
/// \param[in] z dimension along the local Z axis.
///
static InertiaTpl FromBox(const Scalar mass,
const Scalar x,
const Scalar y,
const Scalar z)
{
const Scalar a = mass * (y*y + z*z) / Scalar(12);
const Scalar b = mass * (x*x + z*z) / Scalar(12);
const Scalar c = mass * (y*y + x*x) / Scalar(12);
return InertiaTpl(mass, Vector3::Zero(), Symmetric3(a, Scalar(0), b,
Scalar(0), Scalar(0), c));
}
void setRandom()
......
Markdown is supported
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