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);
        }
    }
}
Ejemplo n.º 3
0
void mtsROSToCISST(const geometry_msgs::Pose & rosData, prmPositionCartesianSet & cisstData)
{
    mtsROSPoseToCISST(rosData, cisstData.Goal());
}