pyinvk 1.0.1
Python Inverse Kinematics.
License: BSD 2-Clause License

# pyinvk

![Alt Text](https://raw.githubusercontent.com/cmower/pyinvk/master/fig8.gif)

`pyinvk` is a library that allows you to setup an inverse kinematic problem with optional constraints.
The package interfaces with several open-source and commerical solvers.

`pyinvk` builds on top of [CasADi](https://web.casadi.org/), and [`urdf2casadi`](https://github.com/mahaarbo/urdf2casadi).
This allows you to compute deriviates of the FK to arbitrary order.
The Jacobian/Hessian's are automatically passed to the desired solvers.

`pyinvk` interfaces with CasADi solvers via `nlpsol` (e.g. IPOPT, SNOPT), and also Scipy optimization routines via the [`scipy.optimize.minimize`](https://docs.scipy.org/doc/scipy/reference/generated/scipy.optimize.minimize.html) method (see *method* parameter for full list of available solvers).
New interfaces can be added by implementing a class that inherits from `Solver` in [`solvers.py`](https://github.com/cmower/pyinvk/blob/master/pyinvk/solver.py).

# Example

Let us specify the inverse kinematic optimization problem with constraints:
```

    min ||eff_pos(q) - eff_pos_goal||^2 + 0.1*||q - qnom||^2
     q

	 s.t.

	 qlo <= q <= qhi
	 eff_pos_z(q) >= 0.1  # keep end-effector z-position above 0.1

```

The following Python code shows how to specify the above problem and call a solver.

```python
import casadi as cs
import numpy as np
from math import radians

from pyinvk.robot_model import RobotModel
from pyinvk.builder import OptimizationBuilder
from pyinvk.solver import CasadiSolver, ScipySolver

# Setup robot model
urdf = 'kuka_lwr.urdf' # urdf file in examples/
tip = 'lwr_arm_7_link'
root = 'base'
robot_model = RobotModel(urdf, root, tip)


# Create optimization builder
N = 1  # number of points in joint trajectory
builder = OptimizationBuilder(robot_model, N)
q = builder.get_q()

# Specify problem
eff_pos = robot_model.get_end_effector_position(q)
eff_pos_goal = builder.add_parameter('eff_pos_goal', 3)
qnom = builder.add_parameter('qnominal', robot_model.ndof)
builder.add_cost_term('goal', cs.sumsqr(eff_pos - eff_pos_goal))
builder.add_cost_term('nominal', 0.1*cs.sumsqr(q - qnom))
builder.enforce_joint_limits()  # specifies limits qlo/qhi as defined in urdf
g = eff_pos[2] - 0.1  # all inequality constraints must be defined such that g >= 0
builder.add_ineq_constraint('zlim', g)

# Build optimization
optimization = builder.build()

# Create solver
Solver = CasadiSolver
#Solver = ScipySolver

solver = Solver(optimization)
solver.setup('ipopt', {})  # pass solver options in {}

# Setup initial seed and parameters
init_seed = np.array([0, radians(30), 0, -radians(90), 0, radians(60), 0])

params = {
  'eff_pos_goal': np.array([0.5, 0.5, 0.5]),
  'qnominal': np.array([0, radians(30), 0, -radians(90), 0, radians(60), 0])
}


# Reset and solve optimization problem
solver.reset(init_seed, params)
solution = solver.solve()

```


# Install

## Pip

```
pip install pyinvk
```

## From source

1. `$ git clone git@github.com:cmower/pyinvk.git`
2. `$ cd pyinvk`
3. `$ pip install .`

# Comparisons

The following plots were generated by running [`examples/expr_pyinvk.py`](https://github.com/cmower/pyinvk/blob/master/example/expr_pyinvk.py). See [here](https://github.com/cmower/pyinvk/blob/master/example/README.md) for details.

![Alt Text](https://raw.githubusercontent.com/cmower/pyinvk/master/example/fig/time.png)

![Alt Text](https://raw.githubusercontent.com/cmower/pyinvk/master/example/fig/err.png)

![Alt Text](https://raw.githubusercontent.com/cmower/pyinvk/master/example/fig/err_eul.png)

![Alt Text](https://raw.githubusercontent.com/cmower/pyinvk/master/example/fig/jdiff.png)