/**
* Create a Joint Trajectory Point Feature Structure from a vector of
* JointState objects.
*
* @param jointStates Source vector of JointState objects.
* @return Joint Trajectory Point with all the information from the objects.
*/
uima::FeatureStructure toJtp(const std::vector<JointState>& jointStates) {
std::size_t size = jointStates.size(), i = 0;
std::vector<JointState>::const_iterator it;
uima::DoubleArrayFS positions = currentCas->createDoubleArrayFS(size);
uima::DoubleArrayFS effort = currentCas->createDoubleArrayFS(size);
uima::DoubleArrayFS velocities = currentCas->createDoubleArrayFS(size);
uima::DoubleArrayFS accelerations = currentCas->createDoubleArrayFS(size);
for (it = jointStates.begin(); it != jointStates.end(); it++, i++) {
positions.set(i, it->position);
effort.set(i, it->effort);
velocities.set(i, it->velocity);
accelerations.set(i, it->acceleration);
}
uima::FeatureStructure jtp = currentCas->createFS(JointTrajectoryPoint);
jtp.setFSValue(jtpPosFtr, positions);
jtp.setFSValue(jtpEffFtr, effort);
jtp.setFSValue(jtpVelFtr, velocities);
jtp.setFSValue(jtpAccFtr, accelerations);
return jtp;
}
/**
* TaskSpacePosition annotations.
*
* @return UIMA error id. UIMA_ERR_NONE on success.
*/
uima::TyErrorId annotateTaskSpace(
uima::CAS& cas,
const urdf::Model& model
) {
uima::FSIndexRepository& index = cas.getIndexRepository();
uima::FeatureStructure ts;
boost::shared_ptr<urdf::Link> link;
std::vector< boost::shared_ptr<urdf::Link> > links;
model.getLinks(links);
for (std::size_t i = 0, size = links.size(); i < size; i++) {
link = links[i];
ts = cas.createFS(TaskSpacePosition);
ts.setFSValue(tsXyzFtr,
utils::toDoubleArrayFS(cas, MongoUrdf::getPosition(link)));
ts.setFSValue(tsRpyFtr,
utils::toDoubleArrayFS(cas, MongoUrdf::getRotation(link)));
index.addFS(ts);
}
return UIMA_ERR_NONE;
}