std::vector<double> Reaching::extractJointPositions(const moveit_msgs::GetPositionIK::Response& ik_response)
{
int num_joints = params_.ik_last_joint_index_ - params_.ik_first_joint_index_ + 1;
std::vector<double> joint_positions(ik_response.solution.joint_state.position.begin() + params_.ik_first_joint_index_,
ik_response.solution.joint_state.position.begin() + params_.ik_first_joint_index_ + num_joints);
return joint_positions;
}
bool BagToCTP::createJointStateTrajectory(dmp_lib::Trajectory& trajectory,
std::vector<ros::Time>& time_stamps,
const std::vector<std::string>& joint_variable_names,
const std::string& abs_bag_file_name,
const std::string& topic_name)
{
if(joint_variable_names.empty())
{
ROS_ERROR("No variable names provided, cannot create trajectory from bag file.");
return false;
}
// read all joint state messages from bag file
std::vector<sensor_msgs::JointState> joint_state_msgs;
ROS_VERIFY(usc_utilities::FileIO<sensor_msgs::JointState>::readFromBagFile(joint_state_msgs, topic_name, abs_bag_file_name));
ROS_INFO("Read >%i< messages from bag file.", (int)joint_state_msgs.size());
if (joint_state_msgs.size() < 2)
{
ROS_ERROR("Too few joint_states messages found to form a trajectory.");
return false;
}
const int num_joints = static_cast<int> (joint_variable_names.size());
const int num_data_points = static_cast<int> (joint_state_msgs.size());
Eigen::VectorXd joint_positions = Eigen::VectorXd::Zero(num_joints);
// initialize trajectory
double dummy_sampling_frequency = 300.0; // real value is set at the end of this function
ROS_VERIFY(trajectory.initialize(joint_variable_names, dummy_sampling_frequency, true, num_data_points));
time_stamps.clear();
// iterate through all messages
for (std::vector<sensor_msgs::JointState>::const_iterator ci = joint_state_msgs.begin(); ci != joint_state_msgs.end(); ++ci)
{
int index = 0;
int num_joints_found = 0;
// for each message, iterate through the list of names
for (std::vector<std::string>::const_iterator vsi = ci->name.begin(); vsi != ci->name.end(); vsi++)
{
// iterate through all variable names
for (int i = 0; i < num_joints; ++i)
{
// find match
// ROS_DEBUG("Comparing: >%s< and >%s<", vsi->c_str(), joint_variable_names[i].c_str());
if(vsi->compare(joint_variable_names[i]) == 0)
{
// ROS_DEBUG("MATCH !");
joint_positions(i) = ci->position[index];
num_joints_found++;
}
}
index++;
}
// check whether all variable names were found
if (num_joints_found != num_joints)
{
ROS_ERROR("Number of joints is >%i<, but there have been only >%i< matches.", num_joints, num_joints_found);
return false;
}
// add data
ROS_VERIFY(trajectory.add(joint_positions));
time_stamps.push_back(ci->header.stamp);
}
input_sampling_frequency_ = (time_stamps.size()-1) / (time_stamps.back() - time_stamps.front()).toSec();
trajectory.setSamplingFrequency(input_sampling_frequency_);
return true;
}
