Tasks Constrained Optimization integration

[ahundt]

The Tasks Constrained Optimization Library will be integrated with grl.

Initial outline on Tasks repository issues:
jrl-umi3218/Tasks#10

Investigation/implementation progress

Working on reading arm kinematics from V-REP and loading them into tasks.

For this we need to translate the dynamics and limits of V-REP into something
tasks understands.

Starting by reading the plugin for converting ROS URDF files into V-REP to see
how joints, limits, and parameters are defined.

It looks like the default revolute joint is:
vJoints.at(i)->jointType==0 // revolute sim_joint_revolute_subtype

This I also tried loading the kuka model from:
https://github.com/SalvoVirga/iiwa_stack/blob/master/iiwa_description/urdf/iiwa14.xacro

This only loaded the joints in but let me see how the transforms work.
The first joint had no RPY offsets, so I assume this would be the identity,
and the translation will only be in the Z axis. From the visualization this
joint rotates around the Z axis, so I believe my Tasks joints should default
to loading Z axis joints us

CONVERSION FUNCTON OUTLINE:
We will want to use
Joint::Type::Rev constructor

plus QuatToE() (need to follow up on jrl-umi3218/RBDyn#16)
This will allow us to

or perhaps
Joint::OldType::RevZ constructor

Requirements:
Will need to use the libraries Tasks, RBDyn, and SpaceVecAlg

Key example:
https://github.com/jrl-umi3218/Tasks/blob/master/tests/arms.h

Key RBDyn files:
https://github.com/jrl-umi3218/RBDyn/blob/master/src/Joint.h

Key functions

Thanks, first I'm working to load the robot model's joints and links from V-REP into Tasks for a 7 dof kuka iiwa 14 R820 arm. There is a model built into V-REP, plus another available in the iiwa_stack library.

The key V-REP functions for the initial setup of just the joint model are:

• simGetJointMatrix()
• simGetJointType()
• simGetShapeMassAndInertia()
• simGetJointPosition()
• simGetJointInterval()
• simGetJointForce()

jrl-umi3218/RBDyn#16

Noncritical

things to keep an eye on
relevant sister french robotics lab to https://github.com/jrl-umi3218:
https://github.com/laas
inspiration for some functionality in RBDyn:
https://github.com/laas/metapod
Old precursor to tasks referenced in method and jrl code:
https://github.com/jrl-umi3218/jrl-dynamics
https://github.com/laas/jrl_dynamics_urdf

[ahundt]

in release 4.0.0