### Run Crocoddyl Examples

Source: https://github.com/loco-3d/crocoddyl/blob/devel/README.md

Execute Crocoddyl examples with display and plot options enabled. Ensure Crocoddyl is installed.

```bash
python -m crocoddyl.examples.quadrupedal_gaits "display" "plot" # enable display and plot
```

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

### Build and Install Crocoddyl from Source

Source: https://github.com/loco-3d/crocoddyl/blob/devel/README.md

Use these commands to build and install Crocoddyl after cloning the repository and creating a build directory. Ensure all mandatory dependencies are installed first.

```bash
cmake .. && make && make install
```

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

### Installing Main Python Binding Files

Source: https://github.com/loco-3d/crocoddyl/blob/devel/bindings/python/crocoddyl/CMakeLists.txt

This loop iterates through the `PROJECT_NAME_PYTHON_BINDINGS_FILES` list, builds each Python file using `python_build`, and then installs them to the designated Python installation directory.

```cmake
foreach(python ${${PROJECT_NAME}_PYTHON_BINDINGS_FILES})
  python_build(. ${python})
  install(
    FILES "${${PROJECT_NAME}_SOURCE_DIR}/bindings/python/crocoddyl/${python}"
    DESTINATION ${CROCODDYL_PYTHON_INSTALL_DIR})
endforeach(python ${${PROJECT_NAME}_PYTHON_BINDINGS_FILES})
```

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

### Installing Python Binding Utility Files

Source: https://github.com/loco-3d/crocoddyl/blob/devel/bindings/python/crocoddyl/CMakeLists.txt

This section defines and installs utility Python files for the bindings. It iterates through a list of utility files, builds them using `python_build`, and installs them into a `utils` subdirectory within the main Python installation path.

```cmake
set(${PROJECT_NAME}_BINDINGS_UTILS_PYTHON_FILES
    __init__.py pendulum.py biped.py humanoid.py quadruped.py)

foreach(python ${${PROJECT_NAME}_BINDINGS_UTILS_PYTHON_FILES})
  python_build(utils ${python})
  install(
    FILES
      "${${PROJECT_NAME}_SOURCE_DIR}/bindings/python/crocoddyl/utils/${python}"
    DESTINATION ${CROCODDYL_PYTHON_INSTALL_DIR}/utils)
endforeach(python ${${PROJECT_NAME}_BINDINGS_UTILS_PYTHON_FILES})
```

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

### Install Notebooks

Source: https://github.com/loco-3d/crocoddyl/blob/devel/notebooks/CMakeLists.txt

Installs the notebook files to the Python site-packages directory, making them accessible after installation.

```cmake
install(FILES ${NOTEBOOK}.ipynb
        DESTINATION ${PYTHON_SITELIB}/${PROJECT_NAME}/notebooks)

```

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

### Build and Run Examples from Build Directory

Source: https://github.com/loco-3d/crocoddyl/blob/devel/README.md

Navigate to the build directory and use make commands to run examples, unit tests, and benchmarks. Input arguments can control display, plotting, and trial parameters.

```bash
cd build
make test
make -s examples-quadrupedal_gaits INPUT="display plot" # enable display and plot
```

```bash
make -s benchmarks-cpp-quadrupedal_gaits INPUT="100 walk" # number of trials ; type of gait
```

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

### Installing Static Files for Python Interface

Source: https://github.com/loco-3d/crocoddyl/blob/devel/bindings/python/crocoddyl/CMakeLists.txt

This command installs static files, such as `.launch` and `.rviz` configuration files, into the Python installation directory. These files are often used for launching ROS nodes or configuring visualization tools.

```cmake
install(
  FILES
    "${${PROJECT_NAME}_SOURCE_DIR}/bindings/python/crocoddyl/crocoddyl.launch"
    "${${PROJECT_NAME}_SOURCE_DIR}/bindings/python/crocoddyl/crocoddyl.rviz"
  DESTINATION ${CROCODDYL_PYTHON_INSTALL_DIR})
```

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

### Setup Robot, Environment, and Target

Source: https://github.com/loco-3d/crocoddyl/blob/devel/notebooks/arm_manipulation.ipynb

Initializes the robot model, sets up the simulation time step and horizon, defines a target position, and configures a visualizer to display the robot and the target in the environment.

```python
import crocoddyl
import example_robot_data
import numpy as np
import pinocchio
import meshcat.geometry as g

robot = example_robot_data.load("talos_arm")
robot_model = robot.model

DT = 1e-3
T = 25
target = np.array([0.4, 0.0, 0.4])

display = crocoddyl.MeshcatDisplay(robot)
display.robot.viewer["world/point"].set_object(g.Sphere(0.05))
display.robot.viewer["world/point"].set_transform(
    np.array(
        [
            [1.0, 0.0, 0.0, target[0]],
            [0.0, 1.0, 0.0, target[1]],
            [0.0, 0.0, 1.0, target[2]],
            [1.0, 0.0, 0.0, 0.0],
        ]
    )
)
```

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

### Setup Optimal Control Problem and Solver

Source: https://github.com/loco-3d/crocoddyl/blob/devel/notebooks/whole_body_manipulation.ipynb

Defines an optimal control problem with integrated action models and solves it using the FDDP solver. It also initializes the display for visualization.

```python
DT, N = 5e-2, 20
target = np.array([0.4, 0, 1.2])

# Creating a running model for the target
running_models = [crocoddyl.IntegratedActionModelEuler(createActionModel(target), DT)] * N
terminal_model = crocoddyl.IntegratedActionModelEuler(createActionModel(target), 0.0)
print("Running models:", running_models[0])
print("Terminal model:", terminal_model)


# Defining the problem and the solver
problem = crocoddyl.ShootingProblem(x0, running_models, terminal_model)
fddp = crocoddyl.SolverFDDP(problem)

# Creating display
display = createDisplay([target])
```

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

### Setup Bipedal Walking Problem

Source: https://github.com/loco-3d/crocoddyl/blob/devel/notebooks/bipedal_walking.ipynb

Initializes the robot model and defines the gait parameters for a bipedal walking problem. Requires the robot model, foot link names, and gait parameters like step length, height, time step, and number of knots.

```python
import example_robot_data
import numpy as np
import pinocchio

import crocoddyl
from crocoddyl.utils.biped import SimpleBipedGaitProblem

# Creating the lower-body part of Talos
talos_legs = example_robot_data.load("talos_legs")

# Setting up the 3d walking problem
rightFoot = "right_sole_link"
leftFoot = "left_sole_link"
gait = SimpleBipedGaitProblem(talos_legs.model, rightFoot, leftFoot)


# Create the initial state
q0 = talos_legs.q0.copy()
v0 = pinocchio.utils.zero(talos_legs.model.nv)
x0 = np.concatenate([q0, v0])


# Creating the walking problem
stepLength = 0.6  # meters
stepHeight = 0.1  # meters
timeStep = 0.0375  # seconds
stepKnots = 20
supportKnots = 10
problem = gait.createWalkingProblem(
    x0, stepLength, stepHeight, timeStep, stepKnots, supportKnots
)


# Solving the 3d walking problem using Feasibility-prone DDP
ddp = crocoddyl.SolverFDDP(problem)

# Using the meshcat displayer, you could enable gepetto viewer for nicer view
# display = crocoddyl.GepettoDisplay(talos_legs, 4, 4)
display = crocoddyl.MeshcatDisplay(talos_legs, 4, 4, False)
ddp.setCallbacks(
    [
        crocoddyl.CallbackLogger(),
        crocodyal.CallbackVerbose(),
        crocoddyl.CallbackDisplay(display),
    ]
)
```

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

### Install Crocoddyl using pip

Source: https://github.com/loco-3d/crocoddyl/blob/devel/README.md

Standard Python package installation using pip. Ensure pip is up-to-date for best results.

```bash
pip install --user crocoddyl
```

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

### Clone Linters Repository and Install Tools

Source: https://github.com/loco-3d/crocoddyl/wiki/Developer-Guide

Clone the Gepetto/linters repository and install necessary formatting tools like clang-format, flake8, isort, and yapf. Note that yapf is in beta.

```bash
git clone git@github.com:Gepetto/linters.git
sudo apt install clang-format-6.0
pip install --user flake8 isort yapf
```

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

### Simplest Optimal Control Example

Source: https://github.com/loco-3d/crocoddyl/blob/devel/doc/Overview.md

This script demonstrates a basic optimal control problem for reaching a goal position with a robot arm. It loads a robot model, defines action models, cost functions, and solves the problem using the DDP solver.

```python
import crocoddyl

import pinocchio
from example_robot_data import load

# Load the robot model
robot_id = "talos"
robot = load(robot_id)

# Create the action model
# The model is defined by the robot dynamics and the cost functions.
# In this case, we use a free-flyer model (no contact) and an Euler sympletic integrator.
# The cost function is defined by the state and control weights.
model = crocoddyl.DifferentialActionModelFreeFwdDynamics(robot.model, crocoddyl.CostModelSum(robot.model, 2))

# Create the action model with an Euler sympletic integrator
# The time step is set to 0.01 seconds.
actuation = crocoddyl.ActuationModelFull(robot.model.nv)
integrator = crocoddyl.IntegratedActionModelEuler(model, actuation, 0.01)

# Create the shooting problem
# The shooting problem is defined by the initial state, the action model, and the terminal cost.
# In this case, the initial state is the robot's zero configuration and velocity.
# The terminal cost is defined by the goal position.
# The number of nodes is set to 250.

# Initial state
x0 = robot.q0

# Terminal cost
terminal_cost = crocoddyl.CostModelResidual(robot.model, crocoddyl.ResidualModelFrameTranslation(robot.model, robot.get_frame_id("end-effector"), pinocchio.utils.zero(3), 0))
terminal_model = crocoddyl.DifferentialActionModelTerminal(crocoddyl.DifferentialActionModelFreeFwdDynamics(robot.model, crocoddyl.CostModelSum(robot.model, 2)))
terminal_model.cost = terminal_cost

# Shooting problem
problem = crocoddyl.ShootingProblem(x0, [integrator] * 250, terminal_model)

# Create the DDP solver
solver = crocoddyl.SolverDDP(problem)

# Solve the optimal control problem
solver.solve()

# Print the optimal trajectory
print(solver.xs)
print(solver.us)

```

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

### Enable Crocoddyl Display and Plotting

Source: https://github.com/loco-3d/crocoddyl/blob/devel/README.md

Set environment variables to enable display and plotting features for Crocoddyl examples. These variables should be exported before running examples.

```bash
export CROCODDYL_DISPLAY=1
```

```bash
export CROCODDYL_PLOT=1
```

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

### Install Crocoddyl on ArchLinux

Source: https://github.com/loco-3d/crocoddyl/blob/devel/README.md

Install Crocoddyl using an AUR helper like paru on ArchLinux.

```bash
paru -Syu crocoddyl
```

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

### Setting Python Installation Directory

Source: https://github.com/loco-3d/crocoddyl/blob/devel/bindings/python/crocoddyl/CMakeLists.txt

This command defines the target installation directory for the Python bindings within the Python site-packages. It constructs the path using the `PYTHON_SITELIB` variable and the project name.

```cmake
set(CROCODDYL_PYTHON_INSTALL_DIR ${PYTHON_SITELIB}/${PROJECT_NAME})
```

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

### Install Crocoddyl with Conda

Source: https://github.com/loco-3d/crocoddyl/blob/devel/README.md

Install Crocoddyl from the conda-forge channel using the conda package manager.

```bash
conda install crocoddyl -c conda-forge
```

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

### Install Crocoddyl with Robotpkg on Debian/Ubuntu

Source: https://github.com/loco-3d/crocoddyl/blob/devel/README.md

Installs Crocoddyl and its Python bindings using the robotpkg package manager on Debian/Ubuntu systems. Requires root privileges.

```bash
sudo apt install robotpkg-py3\*-crocoddyl
```

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

### Create and Solve FDDP Solver with Callbacks

Source: https://github.com/loco-3d/crocoddyl/blob/devel/notebooks/whole_body_manipulation.ipynb

Instantiate the FDDP solver with the defined optimal control problem. Include CallbackVerbose and CallbackDisplay to monitor the solution process and visualize the robot's motion.

```python
# Create the FDDP solver
fddp = crocoddyl.SolverFDDP(problem)
fddp.setCallbacks([crocoddyl.CallbackVerbose(), crocoddyl.CallbackDisplay(display)])

# Solves the problem
print("Problem solved:", fddp.solve())
print("Number of iterations:", fddp.iter)
print("Total cost:", fddp.cost)
print("Gradient norm:", fddp.stoppingCriteria())
```

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

### Visualize and Verify Solution

Source: https://github.com/loco-3d/crocoddyl/blob/devel/notebooks/arm_manipulation.ipynb

Displays the computed trajectory using the visualizer and prints the final gripper position to verify that the robot reached the target.

```python
# Visualizing the solution in gepetto-viewer
display.displayFromSolver(ddp)

robot_data = robot_model.createData()
xT = ddp.xs[-1]
pinocchio.forwardKinematics(robot_model, robot_data, xT[: state.nq])
pinocchio.updateFramePlacements(robot_model, robot_data)
print(
    "Finally reached = ",
    robot_data.oMf[robot_model.getFrameId("gripper_left_joint")].translation.T,
)
```

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

### Initialize and Solve with FDDP

Source: https://github.com/loco-3d/crocoddyl/blob/devel/notebooks/arm_manipulation.ipynb

Initializes the FDDP solver with a defined shooting problem and executes the solve method to find the optimal control trajectory.

```python
ddp = crocoddyl.SolverFDDP(problem)
ddp.solve()
```

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

### Display Trajectory with Meshcat

Source: https://github.com/loco-3d/crocoddyl/blob/devel/notebooks/04_actuating_an_acrobot.ipynb

Visualizes the solved trajectory using Meshcat. Ensure Meshcat is installed and running.

```python
# Display using meshcat
robot_display = crocoddyl.MeshcatDisplay(robot, -1, 1, False)
display(robot_display.robot.viewer.jupyter_cell())
robot_display.displayFromSolver(solver)

```

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

### Initialize Crocoddyl Shooting Problem and Solver

Source: https://github.com/loco-3d/crocoddyl/blob/devel/notebooks/06_scaling_to_robotics.ipynb

Sets up the `ShootingProblem` with the initial state, running models, and terminal model. It then initializes the `SolverFDDP` and sets up logging and visualization callbacks.

```python
# For this optimal control problem, we define 250 knots (or running action
# models) plus a terminal knot
T = 250
problem = crocoddyl.ShootingProblem(x0, [runningModel] * T, terminalModel)

# Creating the DDP solver for this OC problem, defining a logger
solver = crocoddyl.SolverFDDP(problem)
log = crocoddyl.CallbackLogger()

# Using the meshcat displayer, you could enable gepetto viewer for nicer view
# display = crocoddyl.GepettoDisplay(talos_arm, 4, 4)
display = crocoddyl.MeshcatDisplay(talos_arm, 4, 4)
solver.setCallbacks([log, crocoddyl.CallbackVerbose(), crocoddyl.CallbackDisplay(display)])
```

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

### Configure and Format C++ Code

Source: https://github.com/loco-3d/crocoddyl/wiki/Developer-Guide

Set up clang-format by creating a symbolic link to the linter's configuration file in your Crocoddyl source directory. Then, run clang-format to automatically format your C++ code.

```bash
cd ${YOUR_CROCCODDYL_SOURCE_PATH}
ln -s ${YOUR_GEPETTO_LINTER_PATH}/.clang-format-6.0 .clang-format

cd ${YOUR_CROCCODDYL_SOURCE_PATH}
clang-format-6.0 -i $(find . -path ./cmake -prune -o -iregex '.*\.\(h\|c\|hh\|cc\|hpp\|cpp\|hxx\|cxx\)$' -print)
```

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

### Configure Environment Variables for Robotpkg

Source: https://github.com/loco-3d/crocoddyl/blob/devel/README.md

Sets the necessary environment variables to use Crocoddyl installed via robotpkg. Adjust the Python version (e.g., python3.10) if necessary.

```bash
export PATH=/opt/openrobots/bin:$PATH
export PKG_CONFIG_PATH=/opt/openrobots/lib/pkgconfig:$PKG_CONFIG_PATH
export LD_LIBRARY_PATH=/opt/openrobots/lib:$LD_LIBRARY_PATH
export PYTHONPATH=/opt/openrobots/lib/python3.10/site-packages:$PYTHONPATH
```

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

### Re-embed Robot Viewer in Jupyter Cell

Source: https://github.com/loco-3d/crocoddyl/blob/devel/notebooks/whole_body_manipulation.ipynb

Ensure the robot viewer is embedded within the Jupyter cell after defining new targets or modifying the problem setup. This is necessary for visualization.

```python
# Embedded in this cell
display.robot.viewer.jupyter_cell()
```

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

### Configure C++ Formatting

Source: https://github.com/loco-3d/crocoddyl/blob/devel/CONTRIBUTING.md

Link the .clang-format configuration file from the Gepetto linter repository to your Crocoddyl source directory. This ensures C++ code adheres to Google's formatting conventions.

```bash
cd ${YOUR_CROCCODDYL_SOURCE_PATH}
ln -s ${YOUR_GEPETTO_LINTER_PATH}/.clang-format .clang-format
```

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

### Load Acrobot URDF Model

Source: https://github.com/loco-3d/crocoddyl/blob/devel/notebooks/04_actuating_an_acrobot.ipynb

Loads the URDF model of a double pendulum robot using Pinocchio and creates a multibody state representation for Crocoddyl. Ensure the 'example-robot-data' package is installed.

```python
import os
import pathlib

import numpy as np
import pinocchio

# Get the path to the urdf
from example_robot_data.path import EXAMPLE_ROBOT_DATA_MODEL_DIR

import crocoddyl

urdf_model_path = pathlib.Path(
    "double_pendulum_description", "urdf", "double_pendulum_simple.urdf"
)
urdf_model_path = os.path.join(EXAMPLE_ROBOT_DATA_MODEL_DIR, urdf_model_path)

# Now load the model (using pinocchio)
robot = pinocchio.robot_wrapper.RobotWrapper.BuildFromURDF(str(urdf_model_path))

# The model loaded from urdf (via pinicchio)
print(robot.model)

# Create a multibody state from the pinocchio model.
state = crocoddyl.StateMultibody(robot.model)
```

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

### Simulate Rollout

Source: https://github.com/loco-3d/crocoddyl/blob/devel/notebooks/unicycle_towards_origin.ipynb

Computes the system's state trajectory (xs) given an initial state and a sequence of control inputs (us). Requires the plotUnicycle utility for visualization.

```python
us = [np.matrix([1.0, 1.0]).T for _ in range(T)]
xs = problem.rollout(us)
```

```python
%matplotlib inline
from unicycle_utils import plotUnicycle

for x in xs:
    plotUnicycle(x)
plt.axis([-2, 2.0, -2.0, 2.0])
```

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

### Initialize DAMFIC Model

Source: https://github.com/loco-3d/crocoddyl/wiki/Action/Floating-in-Contact-Systems

Instantiate a DifferentialActionModelFloatingInContact with robot dynamics, actuation, contact, and cost models.

```python
damfic = DifferentialActionModelFloatingInContact(pinocchioModel,
                                                  actuationModel,
                                                  contactModel,
                                                  costModel)
```

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

### Run Docker Image for Code Formatting

Source: https://github.com/loco-3d/crocoddyl/wiki/Developer-Guide

Execute the Gepetto/linters Docker image to format C++ code. Ensure the current working directory is accessible by the root user, and use `$(pwd -P)` if symlinks are involved.

```bash
docker run --rm -v $PWD:/root/src -it gepetto/linters --clang-6
```

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

### Configuring Python Library for Scalar Types

Source: https://github.com/loco-3d/crocoddyl/blob/devel/bindings/python/crocoddyl/CMakeLists.txt

This function, `crocoddyl_python_library_for_scalar`, is designed to build Python bindings for a specific scalar type. It maps internal names, defines the Boost.Python wrapper target, links necessary libraries, and handles installation. It's called within a loop to support multiple scalar types.

```cmake
function(crocoddyl_python_library_for_scalar scalar)
  # Map scalar types to internal scalar names
  if("${scalar}" STREQUAL "double")
    set(SCALAR_NAME "float64")
  elseif("${scalar}" STREQUAL "float")
    set(SCALAR_NAME "float32")
  elseif("${scalar}" STREQUAL "CppAD::AD<CppAD::cg::CG<double>>")
    set(SCALAR_NAME "cgfloat64")
  else()
    message(FATAL_ERROR "Unknown scalar type: ${scalar}")
  endif()
  # Define the Boost.Python wrapper target name
  set(PYWRAP "${PROJECT_NAME}_pywrap_${SCALAR_NAME}")
  list(APPEND STUBGEN_DEPENDENCIES "${PYWRAP}")
  # Add the shared library for Boost.Python wrapper
  add_library(${PYWRAP} SHARED ${${PROJECT_NAME}_PYTHON_BINDINGS_SOURCES}
                               ${${PROJECT_NAME}_PYTHON_BINDINGS_HEADERS})
  set_target_properties(${PYWRAP} PROPERTIES SUFFIX ${PYTHON_EXT_SUFFIX})
  # Link the target against the main library and EigenPy
  target_link_libraries(${PYWRAP} ${PROJECT_NAME}::${PROJECT_NAME}
                        eigenpy::eigenpy)
  # Link against pycppad if code generation is enabled
  if(BUILD_WITH_CODEGEN_SUPPORT)
    target_link_libraries(${PYWRAP} pycppad::pycppad)
  endif()
  # Suppress specific compiler warnings for wrapper code
  # BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS spews conversion warnings from int to
  # long unsigned int. Unfortunately, using literals does not work in a macro.
  # As such, this turns them off for the entire wrapper:
  target_compile_options(${PYWRAP} PRIVATE "-Wno-conversion")
  # We are also exposing deprecated declarations until they are removed. Ignore
  # the errors for the wrapper:
  target_compile_options(${PYWRAP} PRIVATE "-Wno-deprecated-declarations")
  # Ensure .pyc files are built before generating stubs
  python_build_get_target(python_build_target)
  add_dependencies(${PYWRAP} ${python_build_target})
  # Set install RPATH on UNIX systems
  if(UNIX)
    get_relative_rpath(${CROCODDYL_PYTHON_INSTALL_DIR} ${PYWRAP}_INSTALL_RPATH)
    set_target_properties(${PYWRAP} PROPERTIES INSTALL_RPATH
                                               "${${PYWRAP}_INSTALL_RPATH}")
  endif()
  # Determine if the scalar type is a floating-point type
  list(FIND SCALAR_FPTYPES "${scalar}" scalar_index)
  if(NOT scalar_index EQUAL -1)
    set(FP_TYPE TRUE)
  else()
    set(FP_TYPE FALSE)
  endif()
  # Generate the scalar-specific C++ files
  crocoddyl_generate_cpp_files(${PYWRAP} cpp bindings/python ${scalar})
  # Install the Boost.Python wrapper target
  install(TARGETS ${PYWRAP} DESTINATION ${CROCODDYL_PYTHON_INSTALL_DIR})
  # Create and install a scalar-specific __init__.py if not float64
  if(NOT "${SCALAR_NAME}" STREQUAL "float64")
    set(SCALAR_TYPE ${SCALAR_NAME})
    configure_file(__init__.py.in
                   ${CMAKE_CURRENT_BINARY_DIR}/${SCALAR_NAME}/__init__.py @ONLY)
    install(FILES ${CMAKE_CURRENT_BINARY_DIR}/${SCALAR_NAME}/__init__.py
            DESTINATION ${CROCODDYL_PYTHON_INSTALL_DIR}/${SCALAR_NAME})
  endif()
endfunction()
```

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

### Solve Optimal Control Problem with SolverFDDP

Source: https://github.com/loco-3d/crocoddyl/blob/devel/notebooks/unicycle_towards_origin.ipynb

Initializes and solves the optimal control problem using the SolverFDDP algorithm. Asserts that the solver converged successfully.

```python
ddp = crocoddyl.SolverFDDP(problem)
done = ddp.solve()
assert done
```

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

### Solve Optimal Control Problem and Print Results

Source: https://github.com/loco-3d/crocoddyl/blob/devel/notebooks/whole_body_manipulation.ipynb

Execute the solver and print key metrics such as status, number of iterations, total cost, and gradient norm. This provides a summary of the optimization outcome.

```python
status = fddp.solve()
print("\nProblem solved:", status, flush=True)
print("Number of iterations:", fddp.iter)
print("Total cost:", fddp.cost)
print("Gradient norm:", fddp.stoppingCriteria())
```

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

### Solve Cartpole Swingup with FDDP

Source: https://github.com/loco-3d/crocoddyl/blob/devel/notebooks/02_optimizing_a_cartpole_swingup.ipynb

Initializes the FDDP solver and sets a verbose callback for logging. Solves the optimal control problem and animates the resulting trajectory.

```python
solver = crocoddyl.SolverFDDP(problem)
solver.setCallbacks([crocoddyl.CallbackVerbose()])
solver.solve()
anim = animateCartpole(solver.xs)
HTML(anim.to_html5_video())
```

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

### Add Callbacks to Inspect Solver Evolution

Source: https://github.com/loco-3d/crocoddyl/blob/devel/notebooks/whole_body_manipulation.ipynb

Use CallbackVerbose to print logs in the terminal and monitor the solver's progress during execution. This is useful for debugging and understanding the optimization process.

```python
fddp.setCallbacks([crocoddyl.CallbackVerbose()])
```

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

### Solve the Optimal Control Problem

Source: https://github.com/loco-3d/crocoddyl/blob/devel/notebooks/arm_manipulation.ipynb

Initializes and runs the FDDP solver on the defined shooting problem. Callbacks are set for verbose output during the solving process.

```python
# Creating the FDDP solver for this OC problem, defining a logger
ddp = crocoddyl.SolverFDDP(problem)
ddp.setCallbacks([crocoddyl.CallbackVerbose()])

# Solving it with the DDP algorithm
ddp.solve()
```

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

### Construct Crocoddyl Problem

Source: https://github.com/loco-3d/crocoddyl/blob/devel/notebooks/04_actuating_an_acrobot.ipynb

Sets up the running and terminal cost models, including state and control costs, and defines the integrated action models using Euler integration. This prepares the problem for solving.

```python
dt = 1e-3  # Time step
T = 1000  # Number of knots

# Cost models
runningCostModel = crocoddyl.CostModelSum(state, nu=actuationModel.nu)
terminalCostModel = crocoddyl.CostModelSum(state, nu=actuationModel.nu)

# Add a cost for the configuration positions and velocities
xref = np.array([0, 0, 0, 0])  # Desired state
stateResidual = crocoddyl.ResidualModelState(state, xref=xref, nu=actuationModel.nu)
stateCostModel = crocoddyl.CostModelResidual(state, stateResidual)
runningCostModel.addCost("state_cost", cost=stateCostModel, weight=1e-5 / dt)
terminalCostModel.addCost("state_cost", cost=stateCostModel, weight=1000)

# Add a cost on control
controlResidual = crocoddyl.ResidualModelControl(state, nu=actuationModel.nu)
bounds = crocoddyl.ActivationBounds(np.array([-1.0]), np.array([1.0]))
activation = crocoddyl.ActivationModelQuadraticBarrier(bounds)
controlCost = crocoddyl.CostModelResidual(
    state, activation=activation, residual=controlResidual
)
runningCostModel.addCost("control_cost", cost=controlCost, weight=1e-1 / dt)

# Create the action models for the state
runningModel = crocoddyl.IntegratedActionModelEuler(
    crocoddyl.DifferentialActionModelFreeFwdDynamics(
        state, actuationModel, runningCostModel
    ),
    dt,
)
terminalModel = crocoddyl.IntegratedActionModelEuler(
    crocoddyl.DifferentialActionModelFreeFwdDynamics(
        state, actuationModel, terminalCostModel
    ),
    0.0,
)
```

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

### Create Meshcat Display

Source: https://github.com/loco-3d/crocoddyl/blob/devel/notebooks/whole_body_manipulation.ipynb

Initializes a Meshcat display for visualizing robot motion and targets. It sets up spheres in the viewer to represent desired target locations.

```python
import meshcat.geometry as g

def createDisplay(targets):
    display = crocoddyl.MeshcatDisplay(robot, 4, 4, False)
    for i, target in enumerate(targets):
        display.robot.viewer["target_" + str(i)].set_object(g.Sphere(0.05))
        display.robot.viewer["target_" + str(i)].set_transform(
            np.array(
                [
                    [1.0, 0.0, 0.0, target[0]],
                    [0.0, 1.0, 0.0, target[1]],
                    [0.0, 0.0, 1.0, target[2]],
                    [0.0, 0.0, 0.0, 1.0],
                ]
            )
        )
    return display
```

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

### Cart-pole Action Data Initialization (Python)

Source: https://github.com/loco-3d/crocoddyl/blob/devel/notebooks/05_codegenerating_a_cartpole.ipynb

Initializes the data structure for the Cart-pole action model in Crocoddyl. This includes setting up variables for state, control, and their derivatives.

```python
def __init__(self, model):
        crocoddyl.cgfloat64.ActionDataAbstract.__init__(self, model)
        self.μ = ADCG(CG(0.0))
        self.ÿ̈ = ADCG(CG(0.0))
        self.θ̈ = ADCG(CG(0.0))
        self.ẏ_next = ADCG(CG(0.0))
        self.θ̇_next = ADCG(CG(0.0))
        self.y_next = ADCG(CG(0.0))
        self.θ_next = ADCG(CG(0.0))
        self.dμ_dθ = ADCG(CG(0.0))
        self.dÿ̈_dy = ADCG(CG(0.0))
        self.dÿ̈_dθ = ADCG(CG(0.0))
        self.dÿ̈_dθ = ADCG(CG(0.0))
        self.dÿ̈_dθ = ADCG(CG(0.0))
        self.dÿ̈_dẏ = ADCG(CG(0.0))
        self.dÿ̈_dθ̇ = ADCG(CG(0.0))
        self.dÿ̈_du = ADCG(CG(0.0))
        self.dθ̈_dy = ADCG(CG(0.0))
        self.dθ̈_dθ = ADCG(CG(0.0))
        self.dθ̈_dθ = ADCG(CG(0.0))
        self.dθ̈_dθ = ADCG(CG(0.0))
        self.dθ̈_dθ = ADCG(CG(0.0))
        self.dθ̈_dẏ = ADCG(CG(0.0))
        self.dθ̈_dθ̇ = ADCG(CG(0.0))
        self.dẏ_next_dy = ADCG(CG(0.0))
        self.dẏ_next_dθ = ADCG(CG(0.0))
        self.dẏ_next_dẏ = ADCG(CG(0.0))
        self.dẏ_next_dθ̇ = ADCG(CG(0.0))
        self.dẏ_next_du = ADCG(CG(0.0))
        self.dθ̇_next_dy = ADCG(CG(0.0))
        self.dθ̇_next_dθ = ADCG(CG(0.0))
        self.dθ̇_next_dẏ = ADCG(CG(0.0))
        self.dθ̇_next_dθ̇ = ADCG(CG(0.0))
        self.dθ̇_next_du = ADCG(CG(0.0))
        self.dy_next_dy = ADCG(CG(0.0))
        self.dy_next_dθ = ADCG(CG(0.0))
        self.dy_next_dẏ = ADCG(CG(0.0))
        self.dy_next_dθ̇ = ADCG(CG(0.0))
        self.dy_next_du = ADCG(CG(0.0))
        self.dθ_next_dy = ADCG(CG(0.0))
        self.dθ_next_dθ = ADCG(CG(0.0))
        self.dθ_next_dẏ = ADCG(CG(0.0))
        self.dθ_next_dθ̇ = ADCG(CG(0.0))
        self.dθ_next_du = ADCG(CG(0.0))
```

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

### Visualize DDP Solution

Source: https://github.com/loco-3d/crocoddyl/blob/devel/notebooks/bipedal_walking.ipynb

Displays the solved optimal control trajectory in the Meshcat visualizer. Sets the display rate and frequency for visualization.

```python
# Visualization of the DDP solution in meshcat
display.rate = -1
display.freq = 1
display.displayFromSolver(ddp)
```

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

### Plotting DDP Convergence and Solution

Source: https://github.com/loco-3d/crocoddyl/blob/devel/notebooks/bipedal_walking.ipynb

Visualizes the results of the DDP solver, including the optimal state and control trajectories, and convergence metrics such as costs, progress, gradients, and steps.

```python
# Plotting the solution and the DDP convergence
log = ddp.getCallbacks()[0]
crocoddyl.plotOCSolution(log.xs, log.us)
crocoddyl.plotConvergence(
    log.costs, log.pregs, log.dregs, log.grads, log.stops, log.steps
)
```

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

### Create Differential Action Model

Source: https://github.com/loco-3d/crocoddyl/blob/devel/notebooks/whole_body_manipulation.ipynb

Builds a differential action model for a robot with double-support contacts and a hand-placement task. Includes state and control regularization, state limits, and friction cone penalization.

```python
def createActionModel(target = None):
    # Creating a double-support contact (feet support)
    contacts = crocoddyl.ContactModelMultiple(state, actuation.nu)
    lf_contact = crocoddyl.ContactModel6D(
        state,
        lf_id,
        pin.SE3.Identity(),
        pin.LOCAL_WORLD_ALIGNED,
        actuation.nu,
        np.array([0, 40.]),
    )
    rf_contact = crocoddyl.ContactModel6D(
        state,
        rf_id,
        pin.SE3.Identity(),
        pin.LOCAL_WORLD_ALIGNED,
        actuation.nu,
        np.array([0, 40.]),
    )
    contacts.addContact("lf_contact", lf_contact)
    contacts.addContact("rf_contact", rf_contact)

    # Define the cost sum (cost manager)
    costs = crocoddyl.CostModelSum(state, actuation.nu)

    # Adding the hand-placement cost
    if target is not None:
        w_hand = np.array([1] * 3 + [0.0001] * 3)
        lh_Mref = pin.SE3(np.eye(3), target)
        activation_hand = crocoddyl.ActivationModelWeightedQuad(w_hand**2)
        lh_cost = crocoddyl.CostModelResidual(
            state,
            activation_hand,
            crocoddyl.ResidualModelFramePlacement(state, lh_id, lh_Mref, actuation.nu),
        )
        costs.addCost("lh_goal", lh_cost, 1e2)

    # Adding state and control regularization terms
    w_x = np.array([0] * 3 + [10.0] * 3 + [0.01] * (state.nv - 6) + [10] * state.nv)
    activation_xreg = crocoddyl.ActivationModelWeightedQuad(w_x**2)
    x_reg_cost = crocoddyl.CostModelResidual(
        state, activation_xreg, crocoddyl.ResidualModelState(state, x0, actuation.nu)
    )
    u_reg_cost = crocoddyl.CostModelResidual(
        state, crocoddyl.ResidualModelControl(state, actuation.nu)
    )
    costs.addCost("xReg", x_reg_cost, 1e-3)
    costs.addCost("uReg", u_reg_cost, 1e-4)

    # Adding the state limits penalization
    x_lb = np.concatenate([state.lb[1 : state.nv + 1], state.lb[-state.nv :]])
    x_ub = np.concatenate([state.ub[1 : state.nv + 1], state.ub[-state.nv :]])
    activation_xbounds = crocoddyl.ActivationModelQuadraticBarrier(
        crocoddyl.ActivationBounds(x_lb, x_ub)
    )
    x_bounds = crocoddyl.CostModelResidual(
        state,
        activation_xbounds,
        crocoddyl.ResidualModelState(state, actuation.nu),
    )
    costs.addCost("xBounds", x_bounds, 1.0)

    # Adding the friction cone penalization
    nsurf, mu = np.identity(3), 0.7
    cone = crocoddyl.FrictionCone(nsurf, mu, 4, False)
    activation_friction = crocoddyl.ActivationModelQuadraticBarrier(
        crocoddyl.ActivationBounds(cone.lb, cone.ub)
    )
    lf_friction = crocoddyl.CostModelResidual(
        state,
        activation_friction,
        crocoddyl.ResidualModelContactFrictionCone(state, lf_id, cone, actuation.nu),
    )
    rf_friction = crocoddyl.CostModelResidual(
        state,
        activation_friction,
        crocoddyl.ResidualModelContactFrictionCone(state, rf_id, cone, actuation.nu),
    )
    costs.addCost("lf_friction", lf_friction, 1e1)
    costs.addCost("rf_friction", rf_friction, 1e1)

    # Creating the action model
    dmodel = crocoddyl.DifferentialActionModelContactFwdDynamics(
        state, actuation, contacts, costs
    )
    return dmodel
```

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

### Run Linters with Docker

Source: https://github.com/loco-3d/crocoddyl/blob/devel/CONTRIBUTING.md

Execute the Gepetto/linters Docker image to format C++ and Python code. Ensure your current working directory is accessible by the root user within the container.

```bash
docker run --rm -v $PWD:/app -w /app -it gepetto/linters
```

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

### Create Crocoddyl Action Models

Source: https://github.com/loco-3d/crocoddyl/blob/devel/notebooks/06_scaling_to_robotics.ipynb

Defines the running and terminal action models using `IntegratedActionModelEuler` with `DifferentialActionModelFreeFwdDynamics`. This sets up the dynamics and costs for each stage of the optimal control problem.

```python
# Running and terminal action models
DT = 1e-3
actuationModel = crocoddyl.ActuationModelFull(state)
runningModel = crocoddyl.IntegratedActionModelEuler(
    crocoddyl.DifferentialActionModelFreeFwdDynamics(
        state, actuationModel, runningCostModel
    ),
    DT,
)
terminalModel = crocoddyl.IntegratedActionModelEuler(
    crocoddyl.DifferentialActionModelFreeFwdDynamics(
        state, actuationModel, terminalCostModel
    ),
    0.0,
)
```

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

### Create and Solve Cart-Pole Shooting Problem

Source: https://github.com/loco-3d/crocoddyl/blob/devel/notebooks/05_codegenerating_a_cartpole.ipynb

Sets up a Cart-Pole shooting problem with a specified number of nodes and solves it using the FDDP solver. Includes visualization of the optimal trajectory.

```python
import sys
from IPython.display import HTML
from cartpole_utils import animateCartpole

# Create a shooting problem with 50 running nodes (i.e., 50 * 5e-2 = 2.5 sec)
N = 50
x0 = np.array([0.0, 0.0, 1.0, 0.5])
problem = crocoddyl.ShootingProblem(x0, [model.copy() for _ in range(N)], model)

# Creating the FDDP solver and setting the logger callback
solver = crocoddyl.SolverFDDP(problem)
solver.setCallbacks([crocoddyl.CallbackVerbose()])

crocoddyl.enable_profiler()

# Solving this problem and display the optimal trajectory of our cart-pole system
solver.solve()

crocoddyl.stop_watch_report(4)
crocoddyl.stop_watch_reset_all()
sys.stdout.flush()

anim = animateCartpole(solver.xs)
# HTML(anim.to_jshtml())
HTML(anim.to_html5_video())
```

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

### Add Robotpkg Repository and Update

Source: https://github.com/loco-3d/crocoddyl/blob/devel/README.md

Commands to add the robotpkg software repository for Debian/Ubuntu and update the package list. Requires root privileges.

```bash
sudo tee /etc/apt/sources.list.d/robotpkg.list <<EOF
 deb [arch=amd64] http://robotpkg.openrobots.org/packages/debian/pub $(lsb_release -sc) robotpkg
 EOF

 curl http://robotpkg.openrobots.org/packages/debian/robotpkg.key | sudo apt-key add -
 sudo apt update
```

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

### Define Shooting Problem and Rollout

Source: https://github.com/loco-3d/crocoddyl/blob/devel/notebooks/04_actuating_an_acrobot.ipynb

Initializes the shooting problem with the initial state and the defined action models. It then performs a simulation rollout with a constant control input and plots the resulting state and control trajectories.

```python
# Define a shooting problem
q0 = np.zeros((state.nq,))
q0[0] = np.pi / 2  # Down
v0 = np.zeros((state.nv,))
# Initial joint velocities
x0 = np.concatenate((q0, v0))  # Inital robot state
problem = crocoddyl.ShootingProblem(x0, [runningModel] * T, terminalModel)

# Test the problem with a rollout
us = [0.01 * np.ones((1,))] * T
xs = problem.rollout(us)

# Handy to blat up the state and control trajectories
crocoddyl.plotOCSolution(xs, us, show=False, figIndex=99, figTitle="Test rollout")
```

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

### Load Robot Model and Initial State

Source: https://github.com/loco-3d/crocoddyl/blob/devel/notebooks/06_scaling_to_robotics.ipynb

Loads a Talos arm robot model using `example_robot_data` and defines an initial state vector (position and velocity).

```python
import numpy as np
import example_robot_data

talos_arm = example_robot_data.load("talos_arm")
robot_model = talos_arm.model  # getting the Pinocchio model

# Defining a initial state
q0 = np.array([0.173046, 1.0, -0.52366, 0.0, 0.0, 0.1, -0.005])
x0 = np.concatenate([q0, np.zeros(talos_arm.model.nv)])
```

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

### Run Python Shell with Crocoddyl using Nix

Source: https://github.com/loco-3d/crocoddyl/blob/devel/README.md

Use this command to enter a Python shell with Crocoddyl pre-installed when using Nix flakes.

```bash
nix run github:loco-3d/crocoddyl
```

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

### Plot Solution and Convergence

Source: https://github.com/loco-3d/crocoddyl/blob/devel/notebooks/06_scaling_to_robotics.ipynb

Generates plots for the optimal control solution (trajectories of states and controls) and the DDP solver's convergence metrics (costs, progress, gradients).

```python
%matplotlib inline
# # Plotting the solution and the DDP convergence
crocoddyl.plotOCSolution(log.xs, log.us)
crocoddyl.plotConvergence(
    log.costs, log.pregs, log.dregs, log.grads, log.stops, log.steps
)

# Visualizing the solution in gepetto-viewer
display.displayFromSolver(solver)
```