In the past there was a method proposed and a discussion about inverse kinematics on the stretch. In short this method works by simplifying the base of the robot to a prismatic joint in the forward/backward direction of the base. This allows for the traditional iterative inverse kinematics to get a configuration from an end effector pose goal. For more details on why that simplification is necessary and how the method workd you can refer to the previous link to read the post and the comments.
There is a different way to get around this problem however and that is via optimization. The goal is to get a whole body configuration from an end effector position. We don’t worry about the trajectory or how to get there via control. Furthermore, the base configuration is modified to represent the planar degrees of freedom x, y and theta instead of the wheel positions. This lends itself to more naturally express goals for the base.
The optimization method used is based on the main example from the OpTaS library which as the authors put it is an “OPtimization-based TAsk Specification library for trajectory optimization and model predictive control”.
As you can see in the problem description, the cost is quadratic and there are nonlinear constraints. What is important about the robot kinematic constraints it that they are twice continuously differentiable, which is important for the solver that is chosen, namely IPOPT.
Stretch urdf modification
For use with the Stretch, we add three virtual joints to the urdf in order to represent the 3 degrees of freedom for planar motion. These are represented by two prismatic and one revolute joints.
Additional cost terms
We penalize the absolute displacement in the y - direction, i.e. laterally wrt to the stretch base wheels forward direction. This does not prevent the movement in the lateral direction but nudges the solver to use the rotation of the base to gain displacement in the y direction.
Furthermore we penalize the displacement of the arm extension from a certain nominal extension. This is because the solver will usually prefer to use the arm extension for every solution and this results in every solution having a fully extended arm. This is not desirable because it puts the robot at the edge of its workspace.
How to run
Download zip file with custom urdf and mesh files for visualization
Install dependency
pip install pyoptas
in order to use the built in visualizer (which is used in the example script in the test_full_body_ik.zip file) you need python 3.8, otherwise you will need to make a visualization of the robot configuration yourself. You can make a virtual environment with python 3.8 if you have a different default version.
To run
- Change the location of the urdf in line 6 of test_optas.py
- Run the test_optas.py script
Notes
The example script only includes goal positions, however this can be extended to goal poses if needed. See OpTaS documentation for more details.
The cost functions are not final but give an example of how the solver can be nudged in the right direction to capture a certain behavior.