Kinematics queries

[adeguet1]

I propose to add two new commands to CRTK. Both would be implemented as services on ROS:

• forward_kinematics
• inverse_kinematics

Proposed new ROS srv can be found in https://github.com/collaborative-robotics/crtk_msgs/tree/devel/srv:

• QueryForwardKinematics.srv: contains jp as input for forward kinematics and cp to help select one of multiple solutions (e.g. for parallel robots).
• QueryInverseKinematics.srv: contains cp as input for inverse kinematics and jp to help select one of multiple solutions.

Both replies contain a bool result and string message for a human readable message if the query failed.