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()); }