void mtsROSToCISST(const cisst_msgs::vctDoubleVec & rosData, prmPositionJointSet & cisstData)
{
const size_t size = rosData.data.size();
cisstData.Goal().resize(size);
for (size_t i = 0; i < size; ++i) {
cisstData.Goal().Element(i) = rosData.data[i];
}
}
void mtsIntuitiveResearchKitArm::SetPositionJoint(const prmPositionJointSet & newPosition)
{
if (CurrentStateIs(mtsIntuitiveResearchKitArmTypes::DVRK_POSITION_JOINT)) {
JointSet.Assign(newPosition.Goal(), NumberOfJoints());
IsGoalSet = true;
}
}
void mtsIntuitiveResearchKitArm::SetPositionGoalJoint(const prmPositionJointSet & newPosition)
{
if (CurrentStateIs(mtsIntuitiveResearchKitArmTypes::DVRK_POSITION_GOAL_JOINT)) {
const double currentTime = this->StateTable.GetTic();
// starting point is last requested to PID component
JointTrajectory.Start.Assign(JointGetDesired, NumberOfJoints());
JointTrajectory.Goal.Assign(newPosition.Goal(), NumberOfJoints());
JointTrajectory.LSPB.Set(JointTrajectory.Start, JointTrajectory.Goal,
JointTrajectory.Velocity, JointTrajectory.Acceleration,
currentTime, robLSPB::LSPB_DURATION);
JointTrajectory.EndTime = currentTime + JointTrajectory.LSPB.Duration();
}
}
void mtsROSToCISST(const sensor_msgs::JointState & rosData, prmPositionJointSet & cisstData)
{
cisstData.Goal().SetSize(rosData.position.size());
std::copy(rosData.position.begin(), rosData.position.end(),
cisstData.Goal().begin());
}
