Tools
Robot Management
- class figaroh.tools.robot.Robot(robot_urdf: str, package_dirs: str, isFext: bool = False, freeflyer_ori: ndarray | None = None, freeflyer_limits: Tuple[float, float] | None = None)[source]
Bases:
RobotWrapperEnhanced Robot class with advanced free-flyer support and visualization utilities.
This class extends RobotWrapper with: - Sophisticated free-flyer configuration and limits - Integrated visualization support - Convenient API for common robot operations - Enhanced error handling and validation
- DEFAULT_FREEFLYER_LIMITS = (-1.0, 1.0)
- property actuated_joint_names: Tuple[str, ...]
Names of actuated joints only.
- display_q0(visualizer_type: str | None = None, q: ndarray | None = None) None[source]
Enhanced visualization with configuration options.
- property freeflyer_limits: Tuple[float, float]
Current free-flyer position limits.
- get_configuration_bounds() Tuple[ndarray, ndarray][source]
Get configuration space bounds with proper handling of quaternions.
- property num_joints: int
Number of actuated joints (excluding universe and free-flyer).
- figaroh.tools.robot.create_robot(robot_urdf: str, package_dirs: str | None = None, loader: str = 'figaroh', enhanced_features: bool = True, **kwargs) Robot | RobotWrapper[source]
Factory function to create robots with optional enhanced features.
- Parameters:
robot_urdf – Robot identifier or URDF path
package_dirs – Package directories
loader – Loader type from load_robot.py
enhanced_features – If True, returns Robot class with enhanced features
**kwargs – Additional arguments for loaders
- Returns:
Robot instance (if enhanced_features=True) or RobotWrapper
- figaroh.tools.robot.get_available_loaders() dict[source]
Get information about available robot loaders.
- Returns:
Information about each loader and its availability
- Return type:
dict
- figaroh.tools.robot.list_available_robots(loader: str = 'robot_description') list[source]
List available robot descriptions for a given loader.
- Parameters:
loader – Loader type to check for available robots
- Returns:
Available robot names
- Return type:
list
- figaroh.tools.robot.load_robot(robot_urdf: str, package_dirs: str | None = None, isFext: bool = False, load_by_urdf: bool = True, robot_pkg: str | None = None, loader: str = 'figaroh', **kwargs) Robot | RobotWrapper | Any[source]
Load robot model from various sources with multiple loader options.
- Parameters:
robot_urdf – Path to URDF file or robot name for robot_description
package_dirs – Package directories for mesh files
isFext – Whether to add floating base joint
load_by_urdf – Whether to load from URDF file (vs ROS param server)
robot_pkg – Name of robot package for path resolution
loader – Loader type - “figaroh”, “robot_description”, “yourdfpy”
**kwargs – Additional arguments passed to the specific loader
- Returns:
“figaroh”: Robot class instance (default, backward compatible)
”robot_description”: RobotWrapper from pinocchio
”yourdfpy”: URDF object from yourdfpy (suitable for viser)
- Return type:
Robot object based on loader type
- Raises:
FileNotFoundError – If URDF file not found
ImportError – If required packages not available
ValueError – If the specified loader is not supported
- Note: For loading by URDF, robot_urdf and package_dirs can be different.
1/ If package_dirs is not provided directly, robot_pkg is used to look up the package directory. 2/ If no mesh files, package_dirs and robot_pkg are not used. 3/ If load_by_urdf is False, the robot is loaded from the ROS parameter server. 4/ For robot_description loader, robot_urdf should be the robot name. 5/ For yourdfpy loader, returns URDF object suitable for viser visualization.
Enhanced Regressor Builder
Regressor matrix computation utilities for robot dynamic identification.
- class figaroh.tools.regressor.RegressorBuilder(robot, config: RegressorConfig | None = None)[source]
Bases:
objectEnhanced regressor builder with better organization.
- class figaroh.tools.regressor.RegressorConfig(has_friction: bool = False, has_actuator_inertia: bool = False, has_joint_offset: bool = False, is_joint_torques: bool = True, is_external_wrench: bool = False, force_torque: List[str] | None = None, additional_columns: int = 4)[source]
Bases:
objectConfiguration for regressor computation.
- additional_columns: int = 4
- force_torque: List[str] | None = None
- has_actuator_inertia: bool = False
- has_friction: bool = False
- has_joint_offset: bool = False
- is_external_wrench: bool = False
- is_joint_torques: bool = True
- figaroh.tools.regressor.build_regressor_basic(robot, q, v, a, identif_config, tau=None)[source]
Legacy function for backward compatibility.
- figaroh.tools.regressor.build_regressor_reduced(W, idx_e)[source]
Build reduced regressor matrix.
- Parameters:
W – Input regressor matrix
idx_e – Indices of columns to eliminate
- Returns:
Reduced regressor matrix
- Return type:
ndarray
- figaroh.tools.regressor.build_total_regressor_current(W_b_u, W_b_l, W_l, I_u, I_l, param_standard_l, identif_config)[source]
Build regressor for total least squares with current measurements.
- Parameters:
W_b_u – Base regressor for unloaded case
W_b_l – Base regressor for loaded case
W_l – Full regressor for loaded case
I_u – Joint currents in unloaded case
I_l – Joint currents in loaded case
param_standard_l – Standard parameters in loaded case
identif_config – Dictionary of settings
- Returns:
Total regressor matrix
Normalized identif_configeter vector
Residual vector
- Return type:
tuple
- figaroh.tools.regressor.build_total_regressor_wrench(W_b_u, W_b_l, W_l, tau_u, tau_l, param_standard_l, param)[source]
Build regressor for total least squares with external wrench measurements.
- Parameters:
W_b_u – Base regressor for unloaded case
W_b_l – Base regressor for loaded case
W_l – Full regressor for loaded case
tau_u – External wrench in unloaded case
tau_l – External wrench in loaded case
param_standard_l – Standard parameters in loaded case
param – Dictionary of settings
- Returns:
Total regressor matrix
Normalized parameter vector
Residual vector
- Return type:
tuple
- figaroh.tools.regressor.eliminate_non_dynaffect(W, params_std, tol_e=1e-06)[source]
Eliminate columns with small L2 norm.
- figaroh.tools.regressor.get_index_eliminate(W, params_std, tol_e=1e-06)[source]
Get indices of columns to eliminate based on tolerance.
- Parameters:
W – Joint torque regressor matrix
params_std – Standard parameters dictionary
tol_e – Tolerance value
- Returns:
List of indices to eliminate
List of remaining parameters
- Return type:
tuple
Robot Visualization
Enhanced robot visualization utilities with better performance.
- class figaroh.tools.robotvisualization.RobotVisualizer(model: Model, data: Data, viz, config: VisualizationConfig | None = None)[source]
Bases:
objectEnhanced robot visualizer with better organization.
- display_axes(q: ndarray, axis_length: float = 0.15, axis_radius: float = 0.01) None[source]
Display coordinate axes for joints.
- display_bounding_boxes(q: ndarray, com_min: ndarray, com_max: ndarray, frame_indices: List[int]) None[source]
Display COM bounding boxes for optimization visualization.
- display_com(q: ndarray, frame_indices: List[int]) None[source]
Display center of mass positions with better naming.
- class figaroh.tools.robotvisualization.VisualizationConfig(com_color: List[float] = None, axes_color: List[float] = None, force_color: List[float] = None, bbox_color: List[float] = None, scale_factor: float = 1.0)[source]
Bases:
objectConfiguration for robot visualization.
- axes_color: List[float] = None
- bbox_color: List[float] = None
- com_color: List[float] = None
- force_color: List[float] = None
- scale_factor: float = 1.0
- figaroh.tools.robotvisualization.display_COM(model: Model, data: Data, viz, q: ndarray, IDX: list) None[source]
Legacy COM display function.
- figaroh.tools.robotvisualization.display_axes(model: Model, data: Data, viz, q: ndarray) None[source]
Legacy axes display function.
- figaroh.tools.robotvisualization.display_bounding_boxes(viz, model: Model, data: Data, q: ndarray, COM_min: ndarray, COM_max: ndarray, IDX: list) None[source]
Legacy bounding box display function.
- figaroh.tools.robotvisualization.display_force(viz, phi: Force, M_se3: SE3) None[source]
Legacy force display function.
- figaroh.tools.robotvisualization.display_joints(viz, model: Model, data: Data, q: ndarray) None[source]
Legacy joint display function.
Robot Collisions
Enhanced collision detection and visualization utilities.
- class figaroh.tools.robotcollisions.CollisionManager(robot, geom_model=None, geom_data=None, viz=None)[source]
Bases:
objectEnhanced collision detection with better performance and safety.
- check_collisions(q: ndarray, update_geometry: bool = True) bool[source]
Check for collisions at given configuration.
- get_collision_distances(collision_details: List | None = None) ndarray[source]
Get minimum distances for collision pairs.
- class figaroh.tools.robotcollisions.CollisionWrapper(robot, geom_model=None, geom_data=None, viz=None)[source]
Bases:
CollisionManagerLegacy wrapper for backward compatibility.
QR Decomposition
QR decomposition utilities for robot parameter identification.
- class figaroh.tools.qrdecomposition.QRDecomposer(tolerance: float = 1e-08, beta_tolerance: float = 1e-06)[source]
Bases:
objectEnhanced QR decomposition handler for robot parameter identification.
- figaroh.tools.qrdecomposition.QR_pivoting(tau: ndarray, W_e: ndarray, params_r: list, tol_qr: float = 1e-08) tuple[source]
Legacy QR pivoting function for backward compatibility.
- figaroh.tools.qrdecomposition.build_baseRegressor(W_e: ndarray, idx_base: tuple) ndarray[source]
Legacy function for building base regressor.
- figaroh.tools.qrdecomposition.cond_num(W_b: ndarray, norm_type: str = None) float[source]
Calculate condition number with various norms.
- figaroh.tools.qrdecomposition.double_QR(tau: ndarray, W_e: ndarray, params_r: list, params_std: dict = None, tol_qr: float = 1e-08) tuple[source]
Legacy double QR function for backward compatibility.
Robot IPOPT
General IPOPT-based optimization framework for robotics applications.
This module provides a comprehensive, flexible framework for setting up and solving nonlinear optimization problems using IPOPT (Interior Point OPTimizer), with specialized support for robotics applications including trajectory optimization, parameter identification, and general robot optimization problems.
- Key Features:
Unified IPOPT interface with robotics-specific configurations
Automatic differentiation support for gradients and Jacobians
Built-in problem validation and result analysis
Specialized classes for trajectory optimization problems
Factory functions for rapid problem setup
Comprehensive logging and iteration tracking
- Main Classes:
IPOPTConfig: Configuration management for IPOPT solver settings
BaseOptimizationProblem: Abstract base class for optimization problems
RobotIPOPTSolver: High-level solver interface with result analysis
TrajectoryOptimizationProblem: Specialized class for trajectory problems
- Usage Examples:
- Basic trajectory optimization:
```python from figaroh.tools.robotipopt import IPOPTConfig, RobotIPOPTSolver
# Create custom problem class class MyProblem(BaseOptimizationProblem):
- def objective(self, x):
return np.sum(x**2) # Minimize sum of squares
- def constraints(self, x):
return np.array([x[0] + x[1] - 1.0]) # x[0] + x[1] = 1
# … implement other required methods
# Solve the problem problem = MyProblem() config = IPOPTConfig.for_trajectory_optimization() solver = RobotIPOPTSolver(problem, config) success, results = solver.solve() ```
- Using the factory function:
```python from figaroh.tools.robotipopt import create_trajectory_solver
- def my_objective(x):
return np.sum(x**2)
- def my_constraints(x):
return np.array([x[0] + x[1] - 1.0])
- def get_bounds():
var_bounds = ([-10, -10], [10, 10]) cons_bounds = ([0], [0]) return var_bounds, cons_bounds
- def initial_guess():
return [0.5, 0.5]
- solver = create_trajectory_solver(
my_objective, my_constraints, get_bounds, initial_guess
- Complete robotics example:
```python import numpy as np from figaroh.tools.robotipopt import (
BaseOptimizationProblem, RobotIPOPTSolver, IPOPTConfig
)
- class RobotTrajectoryProblem(BaseOptimizationProblem):
- def __init__(self, robot, waypoints, active_joints):
super().__init__(“RobotTrajectory”) self.robot = robot self.waypoints = waypoints self.active_joints = active_joints self.n_joints = len(active_joints) self.n_waypoints = len(waypoints) self.n_vars = self.n_joints * self.n_waypoints
- def get_variable_bounds(self):
# Joint limits for all waypoints lb = [-np.pi] * self.n_vars ub = [np.pi] * self.n_vars return lb, ub
- def get_constraint_bounds(self):
# Boundary conditions (start/end positions) n_constraints = 2 * self.n_joints return [0] * n_constraints, [0] * n_constraints
- def get_initial_guess(self):
# Linear interpolation between start and end return np.linspace(
self.waypoints[0], self.waypoints[-1], self.n_waypoints * self.n_joints
)
- def objective(self, x):
# Minimize trajectory smoothness (squared accelerations) q = x.reshape(self.n_waypoints, self.n_joints) acc = np.diff(q, n=2, axis=0) # Second differences return np.sum(acc**2)
- def constraints(self, x):
# Enforce boundary conditions q = x.reshape(self.n_waypoints, self.n_joints) constraints = np.concatenate([
q[0] - self.waypoints[0], # Start position q[-1] - self.waypoints[-1] # End position
]) return constraints
# Usage problem = RobotTrajectoryProblem(robot, waypoints, active_joints) config = IPOPTConfig.for_trajectory_optimization() solver = RobotIPOPTSolver(problem, config)
- if solver.validate_problem_setup():
success, results = solver.solve() if success:
- optimal_trajectory = results[‘x_opt’].reshape(
problem.n_waypoints, problem.n_joints
) print(“Optimization successful!”) print(f”Final objective: {results[‘obj_val’]:.6f}”) print(solver.get_solution_summary())
- Dependencies:
cyipopt: Python interface to IPOPT solver
numpy: Numerical computations
numdifftools: Automatic differentiation (fallback)
References
IPOPT documentation: https://coin-or.github.io/Ipopt/
cyipopt: https://github.com/mechmotum/cyipopt
- class figaroh.tools.robotipopt.BaseOptimizationProblem(name: str = 'OptimizationProblem')[source]
Bases:
ABCAbstract base class for IPOPT optimization problems.
This class defines the interface that all IPOPT problems must implement. Subclasses should implement the objective function, constraints, and their derivatives (or use automatic differentiation).
The class provides default implementations for gradient and Jacobian computation using automatic differentiation via numdifftools. For better performance, subclasses can override these methods with analytical derivatives.
- name
Human-readable name for the optimization problem.
- Type:
str
- logger
Logger instance for this problem.
- Type:
logging.Logger
- iteration_data
Storage for tracking optimization iterations.
- Type:
Dict
- callback_data
Storage for passing data between methods.
- Type:
Dict
- Required Methods (must be implemented by subclasses):
get_variable_bounds(): Return variable bounds as (lower, upper)
get_constraint_bounds(): Return constraint bounds as (lower, upper)
get_initial_guess(): Return initial guess for optimization variables
objective(x): Evaluate objective function at point x
constraints(x): Evaluate constraint functions at point x
- Optional Methods (have default implementations):
gradient(x): Compute objective gradient (uses auto-diff by default)
jacobian(x): Compute constraint Jacobian (uses auto-diff by default)
hessian(x, lagrange, obj_factor): Compute Hessian (IPOPT approx by default)
intermediate(…): Callback for iteration tracking
Examples
- Basic implementation:
```python class QuadraticProblem(BaseOptimizationProblem):
- def get_variable_bounds(self):
return ([-10, -10], [10, 10])
- def get_constraint_bounds(self):
return ([0], [0]) # Equality constraint
- def get_initial_guess(self):
return [1.0, 1.0]
- def objective(self, x):
return x[0]**2 + x[1]**2 # Minimize sum of squares
- def constraints(self, x):
return np.array([x[0] + x[1] - 1.0]) # x[0] + x[1] = 1
- With custom derivatives:
```python class CustomProblem(BaseOptimizationProblem):
# … implement required methods …
- def gradient(self, x):
# Custom analytical gradient return np.array([2*x[0], 2*x[1]])
- def jacobian(self, x):
# Custom analytical Jacobian return np.array([[1.0, 1.0]])
- abstractmethod constraints(x: ndarray) ndarray[source]
Evaluate constraint functions.
- Parameters:
x – Optimization variables
- Returns:
Constraint function values
- abstractmethod get_constraint_bounds() Tuple[List[float], List[float]][source]
Get bounds for constraints.
- Returns:
Tuple of (constraint_lower_bounds, constraint_upper_bounds)
- abstractmethod get_initial_guess() List[float][source]
Get initial guess for optimization variables.
- Returns:
Initial guess as list of floats
- abstractmethod get_variable_bounds() Tuple[List[float], List[float]][source]
Get bounds for optimization variables.
- Returns:
Tuple of (lower_bounds, upper_bounds)
- gradient(x: ndarray) ndarray[source]
Compute gradient of objective function.
Default implementation uses automatic differentiation. Override for custom implementations.
- Parameters:
x – Optimization variables
- Returns:
Gradient vector
- hessian(x: ndarray, lagrange: ndarray, obj_factor: float) bool | ndarray[source]
Compute Hessian of Lagrangian.
Default implementation returns False to use IPOPT’s approximation. Override for custom implementations.
- Parameters:
x – Optimization variables
lagrange – Lagrange multipliers
obj_factor – Objective scaling factor
- Returns:
Hessian matrix or False to use approximation
- intermediate(alg_mod: int, iter_count: int, obj_value: float, inf_pr: float, inf_du: float, mu: float, d_norm: float, regularization_size: float, alpha_du: float, alpha_pr: float, ls_trials: int) bool[source]
Intermediate callback for iteration tracking.
- Parameters:
alg_mod – Algorithm mode
iter_count – Iteration count
obj_value – Current objective value
inf_pr – Primal infeasibility
inf_du – Dual infeasibility
mu – Barrier parameter
d_norm – Step size
regularization_size – Regularization parameter
alpha_du – Dual step size
alpha_pr – Primal step size
ls_trials – Line search trials
- Returns:
True to continue optimization, False to stop
- class figaroh.tools.robotipopt.IPOPTConfig(tolerance: float = 1e-06, acceptable_tolerance: float = 0.0001, max_iterations: int = 3000, max_cpu_time: float = 1000000.0, print_level: int = 5, output_file: str | None = None, hessian_approximation: str = 'limited-memory', warm_start: bool = True, check_derivatives: bool = True, linear_solver: str = 'mumps', custom_options: ~typing.Dict[str, ~typing.Any] = <factory>)[source]
Bases:
objectConfiguration parameters for IPOPT solver.
This class provides a convenient way to set IPOPT solver options with sensible defaults for robotics applications. It includes predefined configurations for common robotics optimization scenarios.
The configuration is designed to work well with: - Trajectory optimization problems (smooth, continuous trajectories) - Parameter identification (high-precision parameter estimation) - General robotics optimization (balanced performance/accuracy)
- tolerance
Primary convergence tolerance for optimization. Default: 1e-6. Smaller values increase precision but runtime.
- Type:
float
- acceptable_tolerance
Fallback tolerance if primary fails. Default: 1e-4. Used when primary tolerance cannot be achieved.
- Type:
float
- max_iterations
Maximum number of optimization iterations. Default: 3000. Increase for complex problems.
- Type:
int
- max_cpu_time
Maximum CPU time in seconds. Default: 1e6. Prevents infinite loops in difficult problems.
- Type:
float
- print_level
IPOPT output verbosity (0-12). Default: 5. Higher values provide more detailed output.
- Type:
int
- output_file
File to save IPOPT output. Default: None. Useful for debugging and analysis.
- Type:
Optional[str]
- hessian_approximation
Hessian computation method. Default: “limited-memory”. Options: “exact”, “limited-memory”.
- Type:
str
- warm_start
Use previous solution as starting point. Default: True. Speeds up subsequent optimizations.
- Type:
bool
- check_derivatives
Enable derivative checking. Default: True. Helps detect implementation errors.
- Type:
bool
- linear_solver
Linear algebra solver backend. Default: “mumps”. Options: “mumps”, “ma27”, “ma57”, “ma77”.
- Type:
str
- custom_options
Additional IPOPT options. Default: {}. For advanced users to set specialized options.
- Type:
Dict[str, Any]
Examples
- Basic usage:
```python # Use default configuration config = IPOPTConfig()
# Customize specific parameters config = IPOPTConfig(
tolerance=1e-8, max_iterations=5000, print_level=3
- Predefined configurations:
```python # For trajectory optimization (speed-focused) config = IPOPTConfig.for_trajectory_optimization()
# For parameter identification (precision-focused) config = IPOPTConfig.for_parameter_identification() ```
- Custom IPOPT options:
```python config = IPOPTConfig(
- custom_options={
“mu_strategy”: “adaptive”, “nlp_scaling_method”: “gradient-based”
}
Note
All string options in custom_options should be provided as strings. They will be automatically encoded to bytes for IPOPT compatibility.
- acceptable_tolerance: float = 0.0001
- check_derivatives: bool = True
- custom_options: Dict[str, Any]
- classmethod for_parameter_identification() IPOPTConfig[source]
Create configuration optimized for parameter identification problems.
This configuration prioritizes high precision over speed, making it suitable for accurate parameter estimation in robotics applications. Uses exact Hessian computation and monotone barrier strategy for numerical stability.
- Returns:
Optimized configuration for parameter identification.
- Return type:
- classmethod for_trajectory_optimization() IPOPTConfig[source]
Create configuration optimized for trajectory optimization problems.
This configuration prioritizes convergence speed over extreme precision, making it suitable for real-time or interactive trajectory planning. Uses adaptive barrier parameter strategy for better convergence.
- Returns:
Optimized configuration for trajectory problems.
- Return type:
- hessian_approximation: str = 'limited-memory'
- linear_solver: str = 'mumps'
- max_cpu_time: float = 1000000.0
- max_iterations: int = 3000
- output_file: str | None = None
- print_level: int = 5
- to_ipopt_options() Dict[str, Any][source]
Convert configuration to IPOPT option dictionary.
Transforms the configuration parameters into the format expected by IPOPT, including proper encoding of string options to bytes.
- Returns:
- Dictionary of IPOPT options ready for use
with cyipopt.Problem.add_option()
- Return type:
Dict[str, Any]
Note
All string values are automatically encoded to bytes as required by the IPOPT C interface via cyipopt.
- tolerance: float = 1e-06
- warm_start: bool = True
- class figaroh.tools.robotipopt.RobotIPOPTSolver(problem: BaseOptimizationProblem, config: IPOPTConfig | None = None)[source]
Bases:
objectGeneral IPOPT solver for robotics optimization problems.
This class provides a high-level interface for solving optimization problems using IPOPT, with built-in support for result analysis, error handling, and problem validation specifically designed for robotics applications.
- Features:
Automatic problem setup and configuration
Built-in problem validation before solving
Comprehensive result analysis and logging
Solution history tracking for multiple solves
Robotics-specific error handling and diagnostics
Support for iterative solving and warm starts
- problem
The optimization problem to solve.
- Type:
- config
IPOPT solver configuration.
- Type:
- logger
Logger for solver operations.
- Type:
logging.Logger
- last_solution
Most recent optimization solution.
- Type:
np.ndarray
- last_info
Most recent IPOPT solver information.
- Type:
Dict
- solve_history
History of all solve attempts.
- Type:
List[Dict]
Examples
- Basic usage:
```python # Create problem and solver problem = MyOptimizationProblem() solver = RobotIPOPTSolver(problem)
# Solve the problem success, results = solver.solve()
- if success:
print(f”Optimal solution: {results[‘x_opt’]}”) print(f”Objective value: {results[‘obj_val’]}”)
- With custom configuration:
```python # Create custom configuration config = IPOPTConfig(
tolerance=1e-8, max_iterations=5000, print_level=3
)
# Create solver with custom config solver = RobotIPOPTSolver(problem, config)
# Validate problem before solving if solver.validate_problem_setup():
success, results = solver.solve()
- Multiple solves with warm start:
```python solver = RobotIPOPTSolver(problem)
# First solve success1, results1 = solver.solve()
# Modify problem parameters… problem.update_parameters(new_params)
# Second solve (uses warm start automatically) success2, results2 = solver.solve()
# Access solve history print(f”Solved {len(solver.solve_history)} problems”) ```
- solve() Tuple[bool, Dict[str, Any]][source]
Solve the optimization problem.
Performs the complete optimization process including problem setup, IPOPT configuration, solving, and result analysis. The method handles all IPOPT interactions and provides comprehensive error handling and logging.
- Returns:
- A tuple containing:
success (bool): True if optimization succeeded, False otherwise
- results (Dict[str, Any]): Dictionary containing:
’x_opt’: Optimal solution vector
’obj_val’: Final objective function value
’status’: IPOPT exit status code
’status_msg’: Human-readable status message
’solve_time’: Total optimization time in seconds
’iterations’: Number of iterations performed
’iteration_data’: Detailed iteration history
’callback_data’: Custom data from problem callbacks
’ipopt_info’: Complete IPOPT solver information
’success’: Copy of success flag for convenience
- Return type:
Tuple[bool, Dict[str, Any]]
- Raises:
Exception – If critical errors occur during problem setup or solving. Non-critical errors are caught and returned in results.
Note
IPOPT status codes for success: -1 (solved to acceptable level), 0 (solved), 1 (solved to acceptable level). All other codes indicate various types of failures or early termination.
- class figaroh.tools.robotipopt.TrajectoryOptimizationProblem(name: str = 'TrajectoryOptimization')[source]
Bases:
BaseOptimizationProblemSpecialized optimization problem for trajectory optimization.
This class provides a framework for trajectory optimization problems with common patterns like waypoint parameterization and constraint handling.
- figaroh.tools.robotipopt.create_trajectory_solver(objective_func: Callable[[ndarray], float], constraint_func: Callable[[ndarray], ndarray], get_bounds_func: Callable[[], Tuple[Tuple[List, List], Tuple[List, List]]], initial_guess_func: Callable[[], List[float]], name: str = 'CustomTrajectory') RobotIPOPTSolver[source]
Factory function to create a trajectory optimization solver.
- Parameters:
objective_func – Function to evaluate objective
constraint_func – Function to evaluate constraints
get_bounds_func – Function that returns ((var_lb, var_ub), (cons_lb, cons_ub))
initial_guess_func – Function that returns initial guess
name – Problem name
- Returns:
Configured IPOPT solver
Random Data Generation
- figaroh.tools.randomdata.generate_waypoints(N, robot, mlow, mhigh)[source]
Generate random values for joint positions, velocities, accelerations.
- Parameters:
N – Number of samples
robot – Robot model object
mlow – Lower bound for random values
mhigh – Upper bound for random values
- Returns:
(q, v, a) joint position, velocity, acceleration arrays
- Return type:
tuple
- figaroh.tools.randomdata.generate_waypoints_fext(N, robot, mlow, mhigh)[source]
Generate random values for joints with external forces.
- Parameters:
N – Number of samples
robot – Robot model object
mlow – Lower bound for random values
mhigh – Upper bound for random values
- Returns:
(q, v, a) joint position, velocity, acceleration arrays
- Return type:
tuple
- figaroh.tools.randomdata.get_torque_rand(N, robot, q, v, a, identif_config)[source]
Calculate random torque values including various dynamic effects.
- Parameters:
N – Number of samples
robot – Robot model object
q – Joint positions array
v – Joint velocities array
a – Joint accelerations array
identif_config – Dictionary of dynamic parameters
- Returns:
Joint torques array
- Return type:
array