void mtsIntuitiveResearchKitMTM::SetPositionCartesian(const prmPositionCartesianSet & newPosition)
{
if (RobotState == mtsIntuitiveResearchKitMTMTypes::MTM_POSITION_CARTESIAN) {
CartesianSetParam = newPosition;
const double currentTime = this->StateTable.GetTic();
// starting point is last requested to PID component
JointTrajectory.Start.Assign(JointGet);
// end point is defined by inverse kinematics but initialize optimizer to start from current
// and try to push L platform away from user's hand
JointTrajectory.Goal.Assign(JointGet);
if (RobotType == MTM_LEFT) {
JointTrajectory.Goal[3] = -cmnPI_4;
} else if (RobotType == MTM_RIGHT) {
JointTrajectory.Goal[3] = cmnPI_4;
}
Manipulator.InverseKinematics(JointTrajectory.Goal, newPosition.Goal());
JointTrajectory.LSPB.Set(JointTrajectory.Start, JointTrajectory.Goal,
JointTrajectory.Velocity, JointTrajectory.Acceleration,
currentTime, robLSPB::LSPB_DURATION);
JointTrajectory.EndTime = currentTime + JointTrajectory.LSPB.Duration();
} else {
CMN_LOG_CLASS_RUN_WARNING << GetName() << ": SetPositionCartesian: MTM not ready" << std::endl;
}
}
void mtsIntuitiveResearchKitArm::SetPositionGoalCartesian(const prmPositionCartesianSet & newPosition)
{
if (CurrentStateIs(mtsIntuitiveResearchKitArmTypes::DVRK_POSITION_GOAL_CARTESIAN)) {
const size_t nbJoints = this->NumberOfJoints();
const size_t nbJointsKin = this->NumberOfJointsKinematics();
const double currentTime = this->StateTable.GetTic();
// copy current position
vctDoubleVec jointSet(JointGet.Ref(nbJointsKin));
// compute desired slave position
CartesianPositionFrm.From(newPosition.Goal());
if (this->InverseKinematics(jointSet, BaseFrame.Inverse() * CartesianPositionFrm) == robManipulator::ESUCCESS) {
// resize to proper number of joints
jointSet.resize(nbJoints);
// keep values of joints not handled by kinematics (PSM jaw)
if (nbJoints > nbJointsKin) {
// check if there is a trajectory active
if (currentTime >= JointTrajectory.EndTime) {
// past any trajectory, use last desired joint position
jointSet.Ref(nbJoints - nbJointsKin,
nbJointsKin).Assign(JointGetDesired.Ref(nbJoints - nbJointsKin,
nbJointsKin));
} else {
// there is an ongoing trajectory, use the trajectory goal
jointSet.Ref(nbJoints - nbJointsKin,
nbJointsKin).Assign(JointTrajectory.Goal.Ref(nbJoints - nbJointsKin,
nbJointsKin));
}
}
const double currentTime = this->StateTable.GetTic();
// starting point is last requested to PID component
JointTrajectory.Start.Assign(JointGetDesired, NumberOfJoints());
JointTrajectory.Goal.Assign(jointSet);
JointTrajectory.LSPB.Set(JointTrajectory.Start, JointTrajectory.Goal,
JointTrajectory.Velocity, JointTrajectory.Acceleration,
currentTime, robLSPB::LSPB_DURATION);
JointTrajectory.EndTime = currentTime + JointTrajectory.LSPB.Duration();
} else {
MessageEvents.Error(this->GetName() + " unable to solve inverse kinematics.");
JointTrajectory.GoalReachedEvent(false);
}
}
}
void mtsROSToCISST(const geometry_msgs::Pose & rosData, prmPositionCartesianSet & cisstData)
{
mtsROSPoseToCISST(rosData, cisstData.Goal());
}