Programmatic API (C++)
From C++ code, it is possible to
create an instance of the
RosClientclass in order to interact with a remotely running instance of CartesIOcreate an instance of the
CartesianInterfaceImplclass in order to actually set up and solve an IK problem, or..
Once one of these objects is created, their API is essentially shared. You can check a reference (doxygen generated) of the C++ API by following this link.
A minimal example (ROS client)
#include <cartesian_interface/ros/RosClient.h>
using namespace XBot::Cartesian;
int main(int argc, char ** argv)
{
RosClient cli;
auto task = cli.getTask("arm1_8");
// ..use task
task->setLambda(0.5);
// ..convert to Cartesian task
auto task_cartesian = std::dynamic_pointer_cast<CartesianTask>(task);
// if conversion was successful...
if(task_cartesian)
{
Eigen::Affine3d Ttgt;
double time = 5.0;
// fill Ttgt...
task_cartesian->getPoseReference(Ttgt);
Ttgt.translation().x() += 0.2;
// command reaching motion
task_cartesian->setPoseTarget(Ttgt, time);
// sleep time
const double sleep_dt = 0.1;
// wait until motion started
while(task_cartesian->getTaskState() == State::Online)
{
cli.update(0, 0);
ros::Duration(sleep_dt).sleep();
}
std::cout << "Motion started" << std::endl;
// wait until motion completed
while(task_cartesian->getTaskState() == State::Reaching)
{
cli.update(0, 0);
ros::Duration(sleep_dt).sleep();
}
std::cout << "Motion completed" << std::endl;
}
}
A minimal CMakeLists.txt to compile the snippet above is the following
cmake_minimum_required(VERSION 3.5)
project(cartesio_example_test)
find_package(catkin REQUIRED COMPONENTS cartesian_interface)
add_compile_options(-std=c++11)
include_directories(${catkin_INCLUDE_DIRS})
add_executable(exampleros exampleros.cpp)
target_link_libraries(exampleros ${catkin_LIBRARIES})
A minimal example (Solver)
An example of embedding the actual solver is way more complex, due mainly to the setup code needed.
Part I: setup
We here retrieve the minimal configuration file set, which is composed of
a URDF file (for this example, we assume it to be available at URDF_PATH)
an SRDF file (SRDF_PATH)
a YAML file with the ik problem description (IK_PB_PATH)
Based on this, we create all required classes, which include:
an
XBot::ModelInterfaceobject to hold the kinematic/dynamic model of the systemthe
XBot::Cartesian::ProblemDescriptionobject which describes the ik problemthe
XBot::Cartesian::CartesianInterfaceImplobject which is the actual solver
#include <cartesian_interface/CartesianInterfaceImpl.h>
#include <thread>
using namespace XBot::Cartesian;
int main(int argc, char **argv)
{
// an option structure which is needed to make a model
XBot::ConfigOptions xbot_cfg;
// set the urdf and srdf path..
xbot_cfg.set_urdf_path(URDF_PATH);
xbot_cfg.set_srdf_path(SRDF_PATH);
// the following call is needed to generate some default joint IDs
xbot_cfg.generate_jidmap();
// some additional parameters..
xbot_cfg.set_parameter("is_model_floating_base", true);
xbot_cfg.set_parameter<std::string>("model_type", "RBDL");
// and we can make the model class
auto model = XBot::ModelInterface::getModel(xbot_cfg);
// initialize to a homing configuration
Eigen::VectorXd qhome;
model->getRobotState("home", qhome);
model->setJointPosition(qhome);
model->update();
// before constructing the problem description, let us build a
// context object which stores some information, such as
// the control period
const double dt = 0.01;
auto ctx = std::make_shared<Context>(
std::make_shared<Parameters>(dt),
model
);
// load the ik problem given a yaml file
auto ik_pb_yaml = YAML::LoadFile(IK_PB_PATH);
ProblemDescription ik_pb(ik_pb_yaml, ctx);
// we are finally ready to make the CartesIO solver "OpenSot"
auto solver = CartesianInterfaceImpl::MakeInstance("OpenSot",
ik_pb, ctx
);
Part II: control loop
We here implement a simplistic finite state machine to command the left_hand
Cartesian task to a target pose, and then exit. At all iterations, it is necessary to:
call
CartesianInterface::update()to compute the ik, which will set the optimized motion (in terms of joint velocity or acceleration) to the modelintegrate such motion to update the joint state
Upon succesfull exit, some .mat files will be available for inspection, as they are auto-generated by CartesIO.
int current_state = 0; // hand-crafted finite state machine!
double time = 0;
Eigen::VectorXd q, qdot, qddot;
while(true)
{
if(current_state == 0) // here we command a reaching motion
{
std::cout << "Commanding left hand forward 0.3m in 3.0 secs" << std::endl;
larm_cartesian->getPoseReference(Tref);
Tref.translation()[0] += 0.3;
double target_time = 3.0;
larm_cartesian->setPoseTarget(Tref, target_time);
current_state++;
}
if(current_state == 1) // here we check that the reaching started
{
if(larm_cartesian->getTaskState() == State::Reaching)
{
std::cout << "Motion started!" << std::endl;
current_state++;
}
}
if(current_state == 2) // here we wait for it to be completed
{
if(larm_cartesian->getTaskState() == State::Online)
{
Eigen::Affine3d T;
larm_cartesian->getCurrentPose(T);
std::cout << "Motion completed, final error is " <<
(T.inverse()*Tref).translation().norm() << std::endl;
current_state++;
}
}
if(current_state == 3) // here we wait the robot to come to a stop
{
std::cout << "qdot norm is " << qdot.norm() << std::endl;
if(qdot.norm() < 1e-3)
{
std::cout << "Robot came to a stop, press ENTER to exit.. \n";
std::cin.ignore();
current_state++;
}
}
if(current_state == 4) break;
// update and integrate model state
solver->update(time, dt);
model->getJointPosition(q);
model->getJointVelocity(qdot);
model->getJointAcceleration(qddot);
q += dt * qdot + 0.5 * std::pow(dt, 2) * qddot;
qdot += dt * qddot;
model->setJointPosition(q);
model->setJointVelocity(qdot);
model->update();
// roughly loop at 100 Hz
std::this_thread::sleep_for(std::chrono::duration<double>(dt));
time += dt;
}
How to compile
find_package(cartesian_interface REQUIRED)
include_directories(${cartesian_interface_INCLUDE_DIRS})
add_executable(cartesio_solver cartesio_solver.cpp)
target_link_libraries(cartesio_solver ${cartesian_interface_LIBRARIES})