### Load Robot Model with Gepetto-Viewer (Python)

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/examples/README.md

Example for loading a robot model using Gepetto-Viewer. Requires gepetto-gui installation.

```bash
python -i gepetto-viewer.py
```

--------------------------------

### Load Model with Basic Geometries using MeshCat (Python)

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/examples/README.md

Example for loading a model with basic geometries using MeshCat. Requires meshcat installation.

```bash
python -i sample-model-viewer.py
```

--------------------------------

### Multiprecision Arithmetic Example (C++)

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/examples/README.md

Example demonstrating the use of multiprecision arithmetic with Pinocchio in C++. Requires building the example.

```bash
./build/multiprecision
```

--------------------------------

### Load Robot Model with MeshCat (Python)

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/examples/README.md

Example for loading a robot model using MeshCat. Requires meshcat installation.

```bash
python -i meshcat-viewer.py
```

--------------------------------

### Build C++ Examples with CMake

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/examples/README.md

Commands to configure and build C++ examples using CMake.

```bash
cmake -B build
cmake --build build
```

--------------------------------

### Minimal C++ CMakeLists.txt for Pinocchio Examples

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/examples/README.md

A minimal CMakeLists.txt file required to build individual C++ examples. Replace 'example1' with the desired example name.

```cmake
cmake_minimum_required(VERSION 3.22)

project(examples CXX)

find_package(pinocchio REQUIRED)

add_executable(example1 example1.cpp)
target_link_libraries(example1 -PUBLIC pinocchio::pinocchio)
```

--------------------------------

### CMake Project Setup and Pinocchio Integration

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/unittest/packaging/cmake/CMakeLists.txt

Configures the CMake project, sets the model directory, and finds the Pinocchio package. Ensure Pinocchio is installed and discoverable by CMake.

```cmake
cmake_minimum_required(VERSION 3.22)

project(ExtraLib CXX)

set(PINOCCHIO_MODEL_DIR "${PROJECT_SOURCE_DIR}/../../../models")
add_definitions('-DPINOCCHIO_MODEL_DIR="${PINOCCHIO_MODEL_DIR}"')

find_package(pinocchio REQUIRED)
```

--------------------------------

### Install Meshcat Viewer

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/examples/README.md

Install the meshcat package using pip for 3D visualization.

```bash
pip install meshcat
```

--------------------------------

### Install Gepetto-Viewer

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/examples/README.md

Install the gepetto-viewer-corba package using conda from the conda-forge channel.

```bash
conda install gepetto-viewer-corba -c conda-forge
```

--------------------------------

### Install Pinocchio Target

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/src/CMakeLists.txt

Installs the main Pinocchio target, including libraries, archives, and runtime components, to their designated installation directories.

```cmake
install(
    TARGETS ${PROJECT_NAME}
    EXPORT ${TARGETS_EXPORT_NAME}
    LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
    ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
    RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}
)
```

--------------------------------

### Install Python Bindings

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/bindings/python/CMakeLists.txt

Installs the Python bindings for Pinocchio. This function handles the installation of shared libraries and Python files.

```cmake
function(INSTALL_PYTHON_BINDINGS)
    install(
        TARGETS ${PYTHON_LIB_NAME}
        EXPORT ${TARGETS_EXPORT_NAME}
        DESTINATION ${PINOCCHIO_PYTHON_INSTALL_DIR}
    )
endfunction()
```

--------------------------------

### Install Panda3D Viewer

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/examples/README.md

Install the panda3d_viewer package using pip.

```bash
pip install panda3d_viewer
```

--------------------------------

### Compute Forward Dynamics Derivatives (Python/C++)

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/examples/README.md

Example for computing forward dynamics derivatives. Execute in Python or build and run the C++ equivalent.

```bash
python -i forward-dynamics-derivatives.py
```

```bash
./build/forward-dynamics-derivatives
```

--------------------------------

### Install Dependencies, Configure, Build, and Test with Pixi

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/development/build.md

Run this command to automate the entire process of setting up, building, and testing the project using Pixi. Dependencies are managed in a local .pixi directory.

```bash
pixi run test
```

--------------------------------

### Finalize Project Setup

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/CMakeLists.txt

This function is called to complete the project setup after all configurations have been made.

```cmake
setup_project_finalize()
```

--------------------------------

### Install Romeo Robot Description Package

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/doc/d-practical-exercises/2-invgeom.md

Installs the necessary package for the Romeo robot model. This is a prerequisite for loading the robot.

```bash
sudo apt install robotpkg-romeo-description
```

--------------------------------

### Running Python Pinocchio Example

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/doc/Overview.md

Command to execute a Python Pinocchio script.

```bash
python overview-urdf.py /path/to/ur5.urdf
```

--------------------------------

### Load URDF Model (Python/C++)

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/examples/README.md

Example demonstrating how to load a URDF model. Execute interactively in Python or build and run the C++ equivalent.

```bash
python -i overview-urdf.py
```

```bash
./build/overview-urdf
```

--------------------------------

### Compiling and Running C++ Pinocchio Example

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/doc/Overview.md

Commands to build and run a C++ Pinocchio application using CMake.

```bash
cmake -B build
cmake --build build
```

```bash
./build/overview-urdf /path/to/ur5.urdf
```

--------------------------------

### Install Header Library Target

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/src/CMakeLists.txt

Installs the header-only library target, including its export configuration.

```cmake
install(
    TARGETS ${PROJECT_NAME}_headers
    EXPORT ${TARGETS_EXPORT_NAME}
    LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
    ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
    RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}
)

```

--------------------------------

### Compute Forward Kinematics Derivatives (Python/C++)

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/examples/README.md

Example for computing forward kinematics derivatives. Execute in Python or build and run the C++ equivalent.

```bash
python -i kinematics-derivatives.py
```

```bash
./build/kinematics-derivatives
```

--------------------------------

### Install Pinocchio via ROS PPA

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/README.md

Install Pinocchio with Coal support and Python bindings using the ROS PPA. Ensure your ROS_DISTRO environment variable is set.

```bash
sudo apt install ros-$ROS_DISTRO-pinocchio
```

--------------------------------

### Define Python Interface Examples

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/examples/CMakeLists.txt

Sets a CMake variable to list examples intended for the Python interface, conditionally appending more examples based on build configurations.

```cmake
set(${PROJECT_NAME}_PYTHON_EXAMPLES
    inverse-kinematics
    inverse-kinematics-3d
    overview-simple
    kinematics-derivatives
    forward-dynamics-derivatives
    inverse-dynamics-derivatives
    model-graph
    model-configuration-converter
    ellipsoid-joint-kinematics
    admm-constraint-solver
)
```

```cmake
if(BUILD_WITH_URDF_SUPPORT)
    list(
        APPEND ${PROJECT_NAME}_PYTHON_EXAMPLES
        overview-urdf
        gepetto-viewer
        build-reduced-model
        robot-wrapper-viewer
        geometry-models
        constraint-cholesky
    )
    if(BUILD_ADVANCED_TESTING)
        # Set as advanced testing because it need a new
        # example_robot_data release (> 4.4)
        # and because computation is long in Debug
        list(
            APPEND ${PROJECT_NAME}_PYTHON_EXAMPLES
            g1-constraint-simulation
        )
    endif()
endif()
```

```cmake
if(BUILD_WITH_COLLISION_SUPPORT)
    list(APPEND ${PROJECT_NAME}_PYTHON_EXAMPLES
        sample-model-viewer
        display-shapes
        simulation-pendulum
        simulation-closed-kinematic-chains
        simulation-lcaba
    )
endif()
```

--------------------------------

### Add Python Examples to List

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/examples/CMakeLists.txt

Appends Python example targets to a project-specific list based on build configurations like URDF support.

```cmake
if(BUILD_WITH_URDF_SUPPORT)
            list(
                APPEND ${PROJECT_NAME}_PYTHON_EXAMPLES
                meshcat-viewer
                meshcat-viewer-dae
                viser-viewer
                collisions
                collision-with-point-clouds
                append-urdf-model-with-another-model
                simulation-contact-dynamics
                # talos-simulation
                static-contact-dynamics
            )
        endif()

        if(BUILD_WITH_OPENMP_SUPPORT)
            list(APPEND ${PROJECT_NAME}_PYTHON_EXAMPLES run-algo-in-parallel)
        endif()
```

--------------------------------

### Load Embedded Model (Python/C++)

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/examples/README.md

Example demonstrating how to load an embedded robot model. Execute interactively in Python or build and run the C++ equivalent.

```bash
python -i overview-simple.py
```

```bash
./build/overview-simple
```

--------------------------------

### Add Pinocchio C++ CasADi Examples

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/examples/casadi/CMakeLists.txt

Conditionally adds Pinocchio C++ examples that utilize CasADi. These examples are added only if CasADi support is enabled and the standalone Python interface is not being built.

```cmake
if(BUILD_WITH_CASADI_SUPPORT)
    if(NOT BUILD_STANDALONE_PYTHON_INTERFACE)
        add_pinocchio_cpp_casadi_example(casadi-aba)
        add_pinocchio_cpp_casadi_example(casadi-crba)
        add_pinocchio_cpp_casadi_example(casadi-rnea)
    endif()

    if(BUILD_PYTHON_INTERFACE)
        set(${PROJECT_NAME}_CASADI_PYTHON_EXAMPLES quadrotor-ocp)
        if(BUILD_WITH_COLLISION_SUPPORT)
            list(APPEND ${PROJECT_NAME}_CASADI_PYTHON_EXAMPLES cartpole)
        endif()

        foreach(EXAMPLE ${${PROJECT_NAME}_CASADI_PYTHON_EXAMPLES})
            set(EXAMPLE_NAME "${PROJECT_NAME}-example-py-casadi-${EXAMPLE}")
            add_python_unit_test(${EXAMPLE_NAME} "examples/casadi/${EXAMPLE}.py" "bindings/python")
            get_test_property(${EXAMPLE_NAME} ENVIRONMENT _ENVIRONMENT)
            set_tests_properties(
                "${EXAMPLE_NAME}"
                PROPERTIES
                    ENVIRONMENT
                        "${_ENVIRONMENT};EXAMPLE_ROBOT_DATA_MODEL_DIR=${EXAMPLE_ROBOT_DATA_MODEL_DIR}"
            )
            add_windows_dll_path_to_test(${EXAMPLE_NAME})
        endforeach()
    endif()
endif()
```

--------------------------------

### Install RViz

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/examples/README.md

Install the ros-noetic-rviz package using conda from the RoboStack channel.

```bash
conda install ros-noetic-rviz -c RoboStack
```

--------------------------------

### Compute Inverse Dynamics Derivatives (Python/C++)

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/examples/README.md

Example for computing inverse dynamics derivatives. Execute in Python or build and run the C++ equivalent.

```bash
python -i inverse-dynamics-derivatives.py
```

```bash
./build/inverse-derivatives
```

--------------------------------

### Simulate Multiple Pendulum (Python)

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/examples/README.md

Example for simulating a multiple pendulum system. Use --with-cart and -N to specify parameters.

```bash
python -i simulation-pendulum.py --with-cart -N=2
```

--------------------------------

### Optimize cost with SciPy optimization tools

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/doc/d-practical-exercises/2-invgeom.md

Demonstrates using SciPy's fmin_bfgs and fmin_slsqp for optimization. Includes examples for unconstrained optimization, and optimization with equality and inequality constraints. Requires SciPy and NumPy.

```python
import numpy as np
from scipy.optimize import fmin_bfgs, fmin_slsqp

def cost(x):
    '''Cost f(x, y) = x² + 2y² - 2xy - 2x '''
    x0, x1 = x
    return -(2 * x0 * x1 + 2 * x0 - x0 ** 2 - 2 * x1 ** 2)

def constraint_eq(x):
    ''' Constraint x³ = y '''
    return np.array([ x[0] ** 3 - x[1] ])

def constraint_ineq(x):
   '''Constraint x >= 2, y >= 2'''
   return np.array([ x[0] - 2, x[1] - 2 ])

class CallbackLogger:
    def __init__(self):
        self.nfeval = 1

    def __call__(self,x):
        print('===CBK=== {0:4d} {1: 3.6f} {2: 3.6f} {3: 3.6f}'.format(self.nfeval, x[0], x[1], cost(x)))
        self.nfeval += 1

x0 = np.array([0.0, 0.0])  # Optimize cost without any constraints in BFGS, with traces.
xopt_bfgs = fmin_bfgs(cost, x0, callback=CallbackLogger())
print('*** Xopt in BFGS =', xopt_bfgs)

# Optimize cost without any constraints in CLSQ

xopt_lsq = fmin_slsqp(cost, [-1.0, 1.0], iprint=2, full_output=1)
print('*** Xopt in LSQ =', xopt_lsq)

# Optimize cost with equality and inequality constraints in CLSQ

xopt_clsq = fmin_slsqp(cost, [-1.0, 1.0], f_eqcons=constraint_eq, f_ieqcons=constraint_ineq, iprint=2, full_output=1)
print('*** Xopt in c-lsq =', xopt_clsq)
```

--------------------------------

### Install Pinocchio with Conda

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/README.md

Use this command to install Pinocchio on your system via the conda-forge channel.

```bash
conda install pinocchio -c conda-forge
```

--------------------------------

### Add Pinocchio C++ Examples

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/examples/CMakeLists.txt

Calls the ADD_PINOCCHIO_CPP_EXAMPLE function to add various C++ examples as unit tests, conditionally based on build flags.

```cmake
add_pinocchio_cpp_example(inverse-kinematics)
add_pinocchio_cpp_example(inverse-kinematics-3d)
add_pinocchio_cpp_example(overview-simple)
add_pinocchio_cpp_example(overview-lie)
add_pinocchio_cpp_example(overview-SE3)
add_pinocchio_cpp_example(interpolation-SE3)
add_pinocchio_cpp_example(model-graph PARSERS)
add_pinocchio_cpp_example(model-configuration-converter PARSERS)
```

```cmake
if(BUILD_WITH_URDF_SUPPORT)
    add_pinocchio_cpp_example(overview-urdf PARSERS)
    add_pinocchio_cpp_example(build-reduced-model PARSERS)
    add_pinocchio_cpp_example(geometry-models PARSERS)
    add_pinocchio_cpp_example(kinematics-derivatives PARSERS)
    add_pinocchio_cpp_example(forward-dynamics-derivatives PARSERS)
    add_pinocchio_cpp_example(inverse-dynamics-derivatives PARSERS)
    if(BUILD_ADVANCED_TESTING)
        add_pinocchio_cpp_example(multiprecision PARSERS)
    endif()
endif()
```

```cmake
if(BUILD_WITH_COLLISION_SUPPORT)
    if(BUILD_WITH_URDF_SUPPORT)
        add_pinocchio_cpp_example(collisions PARSERS)
    endif()
endif()
```

--------------------------------

### Visualize Initial State

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/examples/casadi/simulation-pendulum-variational.ipynb

Displays the initial configuration of the pendulum in the visualizer. This helps to verify the starting pose of the simulation.

```python
q_traj = states_.T
vizer.display(q0)
vizer.viewer.jupyter_cell()

```

--------------------------------

### Use RobotWrapper with URDF (Python)

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/examples/README.md

Example showing how to use RobotWrapper to encapsulate a URDF model in Python.

```bash
python -i robot-wrapper-viewer.py
```

--------------------------------

### Python Collision Detection Examples

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/doc/b-examples/c-collisions.md

Demonstrates various collision detection and distance calculation methods in Python.

```python
from typing import List, Tuple


def distance(p1: Tuple[float, float], p2: Tuple[float, float]) -> float:
    """Calculate the Euclidean distance between two points."""
    return ((p1[0] - p2[0]) ** 2 + (p1[1] - p2[1]) ** 2) ** 0.5


def is_colliding_point_circle(point: Tuple[float, float], circle_center: Tuple[float, float], circle_radius: float) -> bool:
    """Check if a point is colliding with a circle."""
    return distance(point, circle_center) <= circle_radius


def is_colliding_circle_circle(circle1_center: Tuple[float, float], circle1_radius: float, circle2_center: Tuple[float, float], circle2_radius: float) -> bool:
    """Check if two circles are colliding."""
    return distance(circle1_center, circle2_center) <= circle1_radius + circle2_radius


def is_colliding_point_rect(point: Tuple[float, float], rect_min: Tuple[float, float], rect_max: Tuple[float, float]) -> bool:
    """Check if a point is colliding with an axis-aligned rectangle."""
    return rect_min[0] <= point[0] <= rect_max[0] and rect_min[1] <= point[1] <= rect_max[1]


def is_colliding_rect_rect(rect1_min: Tuple[float, float], rect1_max: Tuple[float, float], rect2_min: Tuple[float, float], rect2_max: Tuple[float, float]) -> bool:
    """Check if two axis-aligned rectangles are colliding."""
    return (
        rect1_min[0] <= rect2_max[0] and
        rect1_max[0] >= rect2_min[0] and
        rect1_min[1] <= rect2_max[1] and
        rect1_max[1] >= rect2_min[1]
    )


def closest_point_on_rect(point: Tuple[float, float], rect_min: Tuple[float, float], rect_max: Tuple[float, float]) -> Tuple[float, float]:
    """Find the closest point on an axis-aligned rectangle to a given point."""
    closest_x = max(rect_min[0], min(point[0], rect_max[0]))
    closest_y = max(rect_min[1], min(point[1], rect_max[1]))
    return (closest_x, closest_y)


def distance_point_rect(point: Tuple[float, float], rect_min: Tuple[float, float], rect_max: Tuple[float, float]) -> float:
    """Calculate the distance between a point and an axis-aligned rectangle."""
    closest_point = closest_point_on_rect(point, rect_min, rect_max)
    return distance(point, closest_point)


def is_colliding_circle_rect(circle_center: Tuple[float, float], circle_radius: float, rect_min: Tuple[float, float], rect_max: Tuple[float, float]) -> bool:
    """Check if a circle is colliding with an axis-aligned rectangle."""
    closest_point = closest_point_on_rect(circle_center, rect_min, rect_max)
    return distance(circle_center, closest_point) <= circle_radius


# Example Usage:
point1 = (1.0, 1.0)
point2 = (3.0, 3.0)

circle1_center = (0.0, 0.0)
circle1_radius = 1.0

circle2_center = (1.5, 0.0)
circle2_radius = 1.0

rect1_min = (0.0, 0.0)
rect1_max = (2.0, 2.0)

rect2_min = (1.5, 1.5)
rect2_max = (3.0, 3.0)

print(f"Distance between {point1} and {point2}: {distance(point1, point2):.2f}")
print(f"Is {point1} colliding with circle ({circle1_center}, {circle1_radius})? {is_colliding_point_circle(point1, circle1_center, circle1_radius)}")
print(f"Is circle ({circle1_center}, {circle1_radius}) colliding with circle ({circle2_center}, {circle2_radius})? {is_colliding_circle_circle(circle1_center, circle1_radius, circle2_center, circle2_radius)}")
print(f"Is {point1} colliding with rectangle ({rect1_min}, {rect1_max})? {is_colliding_point_rect(point1, rect1_min, rect1_max)}")
print(f"Is rectangle ({rect1_min}, {rect1_max}) colliding with rectangle ({rect2_min}, {rect2_max})? {is_colliding_rect_rect(rect1_min, rect1_max, rect2_min, rect2_max)}")
print(f"Closest point on rectangle ({rect1_min}, {rect1_max}) to {point2}: {closest_point_on_rect(point2, rect1_min, rect1_max)}")
print(f"Distance between {point2} and rectangle ({rect1_min}, {rect1_max}): {distance_point_rect(point2, rect1_min, rect1_max):.2f}")
print(f"Is circle ({circle1_center}, {circle1_radius}) colliding with rectangle ({rect1_min}, {rect1_max})? {is_colliding_circle_rect(circle1_center, circle1_radius, rect1_min, rect1_max)}")

```

--------------------------------

### Load Romeo Robot Model and Initialize Viewer

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/doc/d-practical-exercises/2-invgeom.md

Loads the URDF model for the Romeo robot and initializes the Gepetto-Viewer. Ensure the robotpkg-romeo-description is installed.

```python
from os.path import join
import pinocchio as se3
from pinocchio.romeo_wrapper import RomeoWrapper

PKG = '/opt/openrobots/share'
URDF = join(PKG, 'romeo_description/urdf/romeo.urdf')

robot = RomeoWrapper(URDF, [PKG])  # Load urdf model
robot.initViewer(loadModel=True)
```

--------------------------------

### Set Include Directories for Headers

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/src/CMakeLists.txt

Configures interface include directories for the header target, including install and build paths.

```cmake
target_include_directories(
    ${PROJECT_NAME}_headers
    INTERFACE
        $<INSTALL_INTERFACE:include>
        $<BUILD_INTERFACE:${PROJECT_BINARY_DIR}/include>
        $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
)

```

--------------------------------

### Set Initial Configuration

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/doc/b-examples/d-inverse-kinematics.md

Define the starting joint configuration for the inverse kinematics algorithm. Using the robot's neutral configuration ensures validity.

```python
q = model.neutral
q = normalize(q)
```

--------------------------------

### Launch Gepetto Viewer

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/doc/d-practical-exercises/1-directgeom.md

Start the Gepetto Viewer, an external program required for robot visualization. This command is typically run in a separate terminal.

```bash
student@student-virtualbox:~$ gepetto-gui
```

--------------------------------

### Initialize Robot Viewer

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/doc/d-practical-exercises/1-directgeom.md

Load the robot's visual model into the Gepetto Viewer. Set loadModel=True when starting or restarting the viewer; subsequent calls can use False.

```python
robot.initViewer(loadModel=True)
```

--------------------------------

### Access footstep plan information

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/doc/d-practical-exercises/6-wpg.md

Example methods for querying the FootSteps object to retrieve phase type, foot positions, phase timing, and support phase transitions.

```python
# Example of use

footsteps.get_phase_type(.4)  # return 'left'
footsteps.get_left_position(0.4)  # return 0,0.1
footsteps.get_left_next_position(0.4)  # return 0.1,0.1
footsteps.get_phase_start(0.4)  # return 0.3
footsteps.get_phase_duration(0.4)  # return 0.7
footsteps.get_phase_remaining(0.4)  # return 0.6
footsteps.is_double_from_left_to_right(0)  # return False
footsteps.is_double_from_left_to_right(1)  # return True
```

--------------------------------

### Check Collisions using Coal (Python/C++)

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/examples/README.md

Example demonstrating collision checking using the Coal library integrated within Pinocchio. Execute in Python or build and run the C++ equivalent.

```bash
python -i collisions.py
```

```bash
./build/collision
```

--------------------------------

### Build and Run Benchmarks

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/benchmark/README.md

Steps to build the benchmark target using pixi and then run a specific benchmark executable.

```bash
pixi shell -e all-benchmark
pixi run configure
cd build
ninja bench
```

```bash
./benchmark/pinocchio-timings-xxx
```

--------------------------------

### Import and Initialize GepettoViewer

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/doc/b-examples/display/a-gepetto-viewer.md

Import the necessary library and initialize GepettoViewer. This is a prerequisite for displaying models.

```python
from gepetto_viewer import Robot

robot = Robot()

```

--------------------------------

### Install Python Files

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/bindings/python/CMakeLists.txt

Installs Python files, optionally within a module subdirectory. This function is used to install Python scripts and modules.

```cmake
function(INSTALL_PYTHON_FILES)
    set(options)
    set(oneValueArgs MODULE)
    set(multiValueArgs FILES)
    cmake_parse_arguments(
        ARGS
        "${options}"
        "${oneValueArgs}"
        "${multiValueArgs}"
        ${ARGN}
    )

    set(SOURCE_PATH ${PROJECT_NAME})
    set(INSTALL_PATH ${PINOCCHIO_PYTHON_INSTALL_DIR})
    if(ARGS_MODULE)
        set(SOURCE_PATH ${SOURCE_PATH}/${ARGS_MODULE})
        set(INSTALL_PATH ${INSTALL_PATH}/${ARGS_MODULE})
    endif()

    foreach(f ${ARGS_FILES})
        python_build(${SOURCE_PATH} ${f})
        install(FILES ${SOURCE_PATH}/${f} DESTINATION ${INSTALL_PATH})
    endforeach()
endfunction()
```

--------------------------------

### Initialize Mobile Robot and Display

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/doc/d-practical-exercises/3-invkine.md

Sets up the UR5 mobile robot, initializes its display, and configures necessary parameters for kinematics. Requires Gepetto server to be running.

```python
import pinocchio as pin
from pinocchio.utils import *
from os.path import dirname, join, abspath
from mobilerobot import MobileRobotWrapper
import time
# Starting gepetto server and give a time
import gepetto.corbaserver

gepetto.corbaserver.start_server()
time.sleep(5)

pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models")
model_path = join(pinocchio_model_dir, "example-robot-data/robots/ur_description")
mesh_dir = pinocchio_model_dir
urdf_filename = "ur5_gripper.urdf"
urdf_model_path = join(join(model_path, "urdf"), urdf_filename)
robot = MobileRobotWrapper(urdf_model_path, pinocchio_model_dir)
robot.initDisplay(loadModel=True)

NQ, NV = robot.model.nq, robot.model.nv

# create valid random position
q = pin.randomConfiguration(robot.model)
robot.display(q)

IDX_TOOL = 24
IDX_BASIS = 23

pin.framesForwardKinematics(robot.model, robot.data, q)
Mtool = robot.data.oMf[IDX_TOOL]
Mbasis = robot.data.oMf[IDX_BASIS]
```

--------------------------------

### Initialize Model and Data Objects

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/doc/b-examples/d-inverse-kinematics.md

Import necessary libraries and create Model and Data objects for inverse kinematics calculations.

```python
import pinocchio as pin
from pinocchio import casadi as cpin
from pinocchio.utils import * 
import numpy as np
import sys

# Load the robot model
model = pin.Model()
data = pin.Data(model)
```

--------------------------------

### Define Pinocchio C++ CasADi Example Function

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/examples/casadi/CMakeLists.txt

Defines a CMake function to add a Pinocchio C++ example that uses CasADi. This is used to streamline the addition of new CasADi-enabled C++ examples.

```cmake
function(ADD_PINOCCHIO_CPP_CASADI_EXAMPLE EXAMPLE)
    add_pinocchio_cpp_example(${EXAMPLE} CASADI)
endfunction()
```

--------------------------------

### Initialize Continuous Pendulum Environment

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/doc/d-practical-exercises/7-learn.md

Sets up the continuous inverted pendulum environment using Pinocchio. Access state via env.x and integrate the simulator with env.step(u).

```python
from pendulum import Pendulum
from pinocchio.utils import *

env = Pendulum(1)  # Continuous pendulum
NX = env.nobs      # ... training converges with q,qdot with 2x more neurones.
NU = env.nu        # Control is dim-1: joint torque

x = env.reset()    # Sample an initial state
u = rand(NU)       # Sample a control
x, reward = env.step(u)   # Integrate simulator for control u and get reward.
env.render()       # Display model at state env.x
```

--------------------------------

### Run Pinocchio Benchmarks and Plot Results

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/benchmark/README.md

Execute benchmarks using `pixi shell`, specify benchmark executable and repetitions, and then plot the output JSON to an HTML file.

```bash
pixi shell -e all-benchmark
cd build
./benchmark/pinocchio-timings-xxx --benchmark_out=output.json --benchmark_repetitions=X
../benchmark/plot.py output.json
```

--------------------------------

### Define C++ Autodiff Example Function

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/examples/cppad/CMakeLists.txt

Defines a CMake function to add C++ examples that use CppAD for automatic differentiation. It includes necessary directories and links required libraries.

```cmake
if(BUILD_WITH_AUTODIFF_SUPPORT AND NOT BUILD_STANDALONE_PYTHON_INTERFACE)
    function(ADD_PINOCCHIO_CPP_AUTODIFF_EXAMPLE EXAMPLE)
        include_directories(SYSTEM ${cppadcg_INCLUDE_DIR})
        add_pinocchio_cpp_example(${EXAMPLE} CPPAD)
        target_link_libraries(
            ${PROJECT_NAME}-example-cpp-${EXAMPLE}
            PUBLIC ${CMAKE_DL_LIBS}
        )
    endfunction()

    add_pinocchio_cpp_autodiff_example(autodiff-rnea)

    if(BUILD_PYTHON_INTERFACE)
        set(${PROJECT_NAME}_AUTODIFF_PYTHON_EXAMPLES autodiff-rnea)

        foreach(EXAMPLE ${${PROJECT_NAME}_AUTODIFF_PYTHON_EXAMPLES})
            set(EXAMPLE_NAME "${PROJECT_NAME}-example-py-${EXAMPLE}")
            add_python_unit_test(${EXAMPLE_NAME} "examples/cppad/${EXAMPLE}.py" "bindings/python")
            add_windows_dll_path_to_test(${EXAMPLE_NAME})
        endforeach()
    endif()
endif()
```

--------------------------------

### Define C++ Example Function

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/examples/CMakeLists.txt

Defines a CMake function to add C++ examples as unit tests, linking necessary libraries and setting compile definitions based on build options.

```cmake
function(ADD_PINOCCHIO_CPP_EXAMPLE EXAMPLE)
    if(NOT BUILD_STANDALONE_PYTHON_INTERFACE)
        set(options PARSERS CPPAD CPPADCG CASADI)
        set(oneValueArgs)
        set(multiValueArgs)
        cmake_parse_arguments(
            example
            "${options}"
            "${oneValueArgs}"
            "${multiValueArgs}"
            ${ARGN}
        )

        get_filename_component(EXAMPLE_NAME ${EXAMPLE} NAME)
        set(EXAMPLE_NAME "${PROJECT_NAME}-example-cpp-${EXAMPLE_NAME}")
        add_unit_test(${EXAMPLE_NAME} "${EXAMPLE}.cpp")
        target_link_libraries(${EXAMPLE_NAME} PUBLIC ${PROJECT_NAME})

        if(example_PARSERS)
            target_link_libraries(
                ${EXAMPLE_NAME}
                PUBLIC ${PROJECT_NAME}_parsers
            )
        endif()

        if(example_CPPAD)
            target_link_libraries(${EXAMPLE_NAME} PUBLIC ${PROJECT_NAME}_cppad)
        endif()

        if(example_CPPADCG)
            target_link_libraries(
                ${EXAMPLE_NAME}
                PUBLIC ${PROJECT_NAME}_cppadcg
            )
        endif()

        if(example_CASADI)
            target_link_libraries(${EXAMPLE_NAME} PUBLIC ${PROJECT_NAME}_casadi)
        endif()

        target_compile_definitions(
            ${EXAMPLE_NAME}
            PRIVATE ${EXAMPLE_PRIVATE_DEFINITIONS}
        )

        modernize_target_link_libraries(
          ${EXAMPLE_NAME}
          SCOPE PRIVATE
          TARGETS Boost::filesystem
          LIBRARIES ${Boost_FILESYSTEM_LIBRARY}}
          INCLUDE_DIRS ${Boost_INCLUDE_DIRS}
        )

        # There is no RPATH in Windows, then we must use the PATH to find the DLL
        if(WIN32)
            string(REPLACE ";" "\\\;" _PATH "$ENV{PATH}")
            set(ENV_VARIABLES
                "PATH=${_PATH}\\;${PROJECT_BINARY_DIR}/src\\
;${PROJECT_BINARY_DIR}/bindings/python/pinocchio"
            )
            set_tests_properties(
                ${EXAMPLE_NAME}
                PROPERTIES ENVIRONMENT "${ENV_VARIABLES}"
            )
        endif()
        if(TARGET example-robot-data::example-robot-data)
            target_link_libraries(
                ${EXAMPLE_NAME}
                PRIVATE example-robot-data::example-robot-data
            )
        else()
            target_compile_definitions(
                ${EXAMPLE_NAME}
                PRIVATE
                    "EXAMPLE_ROBOT_DATA_MODEL_DIR=\"${EXAMPLE_ROBOT_DATA_MODEL_DIR}\""
            )
        endif()
    endif()
endfunction()
```

--------------------------------

### Define C++ Codegen Example Function

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/examples/codegen/CMakeLists.txt

Defines a CMake function to add C++ examples that utilize code generation. This function is conditional on CppADCG, code generation support, and URDF support being enabled.

```cmake
if(cppadcg_FOUND AND BUILD_WITH_CODEGEN_SUPPORT AND BUILD_WITH_URDF_SUPPORT)
    function(ADD_PINOCCHIO_CPP_CODEGEN_EXAMPLE EXAMPLE)
        add_pinocchio_cpp_example(
          ${EXAMPLE}
          CPPADCG
          PARSERS
        )
        target_link_libraries(
            ${PROJECT_NAME}-example-cpp-${EXAMPLE}
            PUBLIC ${CMAKE_DL_LIBS}
        )
        target_compile_definitions(
            ${PROJECT_NAME}-example-cpp-${EXAMPLE}
            PUBLIC PINOCCHIO_CXX_COMPILER="${CMAKE_CXX_COMPILER}"
        )
    endfunction()

    if(NOT BUILD_STANDALONE_PYTHON_INTERFACE)
        add_pinocchio_cpp_codegen_example(codegen-crba)
        add_pinocchio_cpp_codegen_example(codegen-rnea)
    endif()

    if(BUILD_PYTHON_INTERFACE)
        set(${PROJECT_NAME}_CODEGEN_PYTHON_EXAMPLES codegen-rnea)

        foreach(EXAMPLE ${${PROJECT_NAME}_CODEGEN_PYTHON_EXAMPLES})
            set(EXAMPLE_NAME "${PROJECT_NAME}-example-py-${EXAMPLE}")
            add_python_unit_test(${EXAMPLE_NAME} "examples/codegen/${EXAMPLE}.py" "bindings/python")
            add_windows_dll_path_to_test(${EXAMPLE_NAME})
        endforeach()
    endif()
endif()
```

--------------------------------

### Initialize Discrete Pendulum Environment

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/doc/d-practical-exercises/7-learn.md

Sets up the discrete inverted pendulum environment. State is an integer from 0 to NQ*NV, and controls are discretized from 0 to NU.

```python
from dpendulum import DPendulum

env = DPendulum()
NX = env.nx  # Number of (discrete) states
NU = env.nu  # Number of (discrete) controls
env.reset()
env.step(5)
env.render()
```

--------------------------------

### Solve Quadratic Program

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/doc/d-practical-exercises/4-dyn.md

Demonstrates the usage of the solve_qp function from the quadprog library to solve a quadratic programming problem.

```python
from quadprog import solve_qp

# Solve min_x .5 xHx - gx s.t. Cx <= d
x, _, _, _, _, _ = solve_qp(H, g, C, d)
```

--------------------------------

### C++ Collision Detection Examples

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/doc/b-examples/c-collisions.md

Provides C++ implementations for collision detection and distance calculations.

```cpp
#include <cmath>
#include <iostream>
#include <vector>

struct Point {
    double x, y;
};

struct Circle {
    Point center;
    double radius;
};

struct Rect {
    Point min;
    Point max;
};

// Calculate the Euclidean distance between two points.
double distance(Point p1, Point p2) {
    return std::sqrt(std::pow(p1.x - p2.x, 2) + std::pow(p1.y - p2.y, 2));
}

// Check if a point is colliding with a circle.
bool is_colliding_point_circle(Point point, Circle circle) {
    return distance(point, circle.center) <= circle.radius;
}

// Check if two circles are colliding.
bool is_colliding_circle_circle(Circle c1, Circle c2) {
    return distance(c1.center, c2.center) <= c1.radius + c2.radius;
}

// Check if a point is colliding with an axis-aligned rectangle.
bool is_colliding_point_rect(Point point, Rect rect) {
    return point.x >= rect.min.x && point.x <= rect.max.x &&
           point.y >= rect.min.y && point.y <= rect.max.y;
}

// Check if two axis-aligned rectangles are colliding.
bool is_colliding_rect_rect(Rect r1, Rect r2) {
    return r1.min.x <= r2.max.x && r1.max.x >= r2.min.x &&
           r1.min.y <= r2.max.y && r1.max.y >= r2.min.y;
}

// Find the closest point on an axis-aligned rectangle to a given point.
Point closest_point_on_rect(Point point, Rect rect) {
    Point closest;
    closest.x = std::max(rect.min.x, std::min(point.x, rect.max.x));
    closest.y = std::max(rect.min.y, std::min(point.y, rect.max.y));
    return closest;
}

// Calculate the distance between a point and an axis-aligned rectangle.
double distance_point_rect(Point point, Rect rect) {
    Point closest = closest_point_on_rect(point, rect);
    return distance(point, closest);
}

// Check if a circle is colliding with an axis-aligned rectangle.
bool is_colliding_circle_rect(Circle circle, Rect rect) {
    Point closest = closest_point_on_rect(circle.center, rect);
    return distance(circle.center, closest) <= circle.radius;
}

int main() {
    Point p1 = {1.0, 1.0};
    Point p2 = {3.0, 3.0};

    Circle c1 = {{0.0, 0.0}, 1.0};
    Circle c2 = {{1.5, 0.0}, 1.0};

    Rect r1 = {{0.0, 0.0}, {2.0, 2.0}};
    Rect r2 = {{1.5, 1.5}, {3.0, 3.0}};

    std::cout << "Distance between p1 and p2: " << distance(p1, p2) << std::endl;
    std::cout << "Is p1 colliding with c1? " << std::boolalpha << is_colliding_point_circle(p1, c1) << std::endl;
    std::cout << "Is c1 colliding with c2? " << std::boolalpha << is_colliding_circle_circle(c1, c2) << std::endl;
    std::cout << "Is p1 colliding with r1? " << std::boolalpha << is_colliding_point_rect(p1, r1) << std::endl;
    std::cout << "Is r1 colliding with r2? " << std::boolalpha << is_colliding_rect_rect(r1, r2) << std::endl;
    Point closest = closest_point_on_rect(p2, r1);
    std::cout << "Closest point on r1 to p2: (" << closest.x << ", " << closest.y << ")" << std::endl;
    std::cout << "Distance between p2 and r1: " << distance_point_rect(p2, r1) << std::endl;
    std::cout << "Is c1 colliding with r1? " << std::boolalpha << is_colliding_circle_rect(c1, r1) << std::endl;

    return 0;
}

```

--------------------------------

### Initialize and Display Pendulum Model

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/examples/casadi/simulation-pendulum-variational.ipynb

Initializes the pendulum model and visualizer, then displays the default configuration in the viewer.

```python
model, geom_model = makeModel()
model
```

```python
from pinocchio.visualize import MeshcatVisualizer
```

```python
vizer = MeshcatVisualizer(model, geom_model, visual_model=geom_model)
```

```python
vizer.initViewer()
vizer.loadViewerModel("pinocchio")
```

```python
q0 = pin.neutral(model)
vizer.display(q0)
```

```python
vizer.viewer.jupyter_cell()
```

--------------------------------

### Instantiate SE(2) Objects in C++

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/doc/a-features/e-lie.md

Demonstrates how to instantiate SE(2) objects for representing robot poses in a 2D plane. Supports custom scalar types like float.

```cpp
typedef double Scalar;
enum {Options = 0};

SpecialEuclideanOperationTpl<2,Scalar,Options> aSE2;
SpecialEuclideanOperationTpl<2,Scalar,Options>::ConfigVector_t pose_s,pose_g;
SpecialEuclideanOperationTpl<2,Scalar,Options>::TangentVector_t delta_u;
```

--------------------------------

### Display a model using Meshcat

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/doc/b-examples/display/b-meshcat-viewer.md

This Python script visualizes a URDF model using the Meshcat viewer. Ensure Meshcat is installed and running.

```python
import meshcat.geometry as g
import meshcat.linalg as linalg
import meshcat.transformations as tf
import meshcat


viewer = meshcat.Visualizer()

# Load a URDF model
# You can replace this with your own URDF file path
# For example: viewer.open("file:///path/to/your/model.urdf")
viewer.open("file:///opt/homebrew/lib/python3.9/site-packages/pinocchio/models/atlas/atlas.urdf")

# Add a simple sphere to the scene
viewer.set_object(g.Sphere(0.1), meshcat.Red())
```

--------------------------------

### Run Include-What-You-Use on Private Headers

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/development/convention.md

This command is similar to the C++ file analysis but includes the -DPINOCCHIO_LSP define, suggesting it's tailored for analyzing private headers in conjunction with language server protocol support. It uses custom mapping files for Eigen, Casadi, and Boost.

```bash
include-what-you-use -isystem $CONDA_PREFIX/include/eigen3 \
   -isystem $CONDA_PREFIX/include \
   -I include \
   -I build/include \
   -DPINOCCHIO_LSP \
   -DPINOCCHIO_WITH_COLLISION \
   -DBOOST_MPL_CFG_NO_PREPROCESSED_HEADERS \
   -DBOOST_MPL_LIMIT_LIST_SIZE=30 \
   -DBOOST_MPL_LIMIT_VECTOR_SIZE=30 \
   -DBOOST_FUSION_INVOKE_MAX_ARITY=12 \
   -Xiwyu --mapping_file=development/iwyu/eigen.imp \
   -Xiwyu --mapping_file=development/iwyu/casadi.imp \
   -Xiwyu --mapping_file=development/iwyu/boost.imp \
   -Xiwyu --max_line_length=120\
   unittest/spatial.cpp
```

--------------------------------

### Fetch and Configure Pybind11

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/unittest/python/pybind11/CMakeLists.txt

Fetches pybind11 using FetchContent if not already found. Ensures pybind11::module target is available.

```cmake
find_package(pybind11 CONFIG QUIET)
if(NOT TARGET pybind11::module)
    include(FetchContent)
    FetchContent_Declare(
        pybind11
        GIT_REPOSITORY https://github.com/pybind/pybind11
        GIT_TAG v3.0.1
    )
    FetchContent_GetProperties(pybind11)
    if(NOT pybind11_POPULATED)
        FetchContent_Populate(pybind11)
        add_subdirectory(${pybind11_SOURCE_DIR} ${pybind11_BINARY_DIR})
    endif()
endif()
```

--------------------------------

### Factor Graph for Optimization

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/doc/d-practical-exercises/6-wpg.md

This snippet demonstrates how to set up and solve a factor graph for optimization problems. It's useful for defining constraints and costs on variables.

```python
f = FactorGraph(1, 5)  # Define a factor of 5 variables of dimension 1

M = eye(1)  # M is simply 1 written as a 1x1 matrix.
for i in range(4):
    f.add_factor_constraint([Factor(i, M), Factor(i + 1, -M)], zero(1))

f.addFactor([Factor(0, M)], M * 10)
f.addFactor([Factor(4, M)], M * 20)

x = f.solve()
```

--------------------------------

### Manually Build Project with CMake and Ninja

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/development/build.md

Access the Pixi shell to manually configure and build the project using CMake and Ninja. This is an alternative to the automated `pixi run test` command.

```bash
pixi shell
cmake --build build
ninja
```

--------------------------------

### Get Current Tool Placement

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/doc/d-practical-exercises/3-invkine.md

Computes the forward kinematics and updates frame placements to retrieve the current SE(3) placement of the tool frame.

```python
pin.forwardKinematics(robot.model, robot.data, q)  # Compute joint placements
pin.updateFramePlacements(robot.model, robot.data)      # Also compute operational frame placements
Mtool = robot.data.oMf[IDX_TOOL]  # Get placement from world frame o to frame f oMf
```

--------------------------------

### Find and Link Pinocchio Headers in CMake

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/unittest/packaging/pinocchio_header/CMakeLists.txt

Use this snippet to find the Pinocchio package and link its headers to your executable. Ensure Pinocchio is installed and discoverable by CMake.

```cmake
cmake_minimum_required(VERSION 3.22)

project(ExtraLib CXX)
find_package(pinocchio REQUIRED)

add_executable(run_fk ../run_fk.cpp)
target_link_libraries(run_fk PUBLIC pinocchio::pinocchio_headers)
```

--------------------------------

### Run Pinocchio Docker Image

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/README.md

This command launches a Docker container with the Pinocchio development environment.

```bash
docker run --rm -it ghcr.io/stack-of-tasks/pinocchio:devel
```

--------------------------------

### Constrain OpenMP Parallel Execution

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/benchmark/README.md

Example of setting environment variables to constrain OpenMP to specific CPU cores for parallel benchmarks, ensuring consistent performance.

```bash
GOMP_CPU_AFFINITY="0-15:2" OMP_PROC_BIND=TRUE ./benchmark/pinocchio-timings-parallel
```

--------------------------------

### Initialize Robot Hand Model

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/doc/d-practical-exercises/4-dyn.md

Initializes the robot hand model and displays its initial configuration.

```python
from robot_hand import Robot

robot = Robot()
robot.display(robot.q0)
```

--------------------------------

### Access and Get Visual Object Properties

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/doc/d-practical-exercises/1-directgeom.md

Retrieve specific visual geometry objects from the robot model and access their properties like name and viewer reference.

```python
visualObj = robot.visual_model.geometryObjects[4]  # 3D object representing the robot forarm
visualName = visualObj.name                        # Name associated to this object
visualRef = robot.getViewerNodeName(visualObj, pin.GeometryType.VISUAL)    # Viewer reference (string) representing this object
```

--------------------------------

### Initialize and Display Robot Hand

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/doc/d-practical-exercises/4-dyn.md

Initializes the robot hand and displays its initial configuration using the display method.

```python
from robot_hand import Robot
robot = Robot()
robot.display(robot.q0)
```

--------------------------------

### Get Joint Placement

Source: https://github.com/stack-of-tasks/pinocchio/blob/devel/doc/d-practical-exercises/1-directgeom.md

Retrieve the placement of a specific joint in the robot's configuration. Remember to copy the placement object to avoid unintended side effects.

```python
# Get index of end effector

idx = robot.index('wrist_3_joint')

# Compute and get the placement of joint number idx

placement = robot.placement(q, idx)
# Be carreful, Python always returns references to values.
# You can often .copy() the object to avoid side effects
# Only get the placement
placement = robot.data.oMi[idx].copy()
```