Exemplo n.º 1
0
 void ControlQueue::switchMode(int mode) {
     if(ros::ok) {
         if(!isShutUp()) {
             ROS_INFO("(ControlQueue) switching control mode");
         }
         setCurrentControlTypeInternal(mode);
         currentControlType = mode;
     } else {
         if(!isShutUp())
             ROS_INFO("(ControlQueue) ros error");
     }
 }
Exemplo n.º 2
0
    void KukieControlQueue::setStiffness(float cpstiffnessxyz, float cpstiffnessabc, float cpdamping, float cpmaxdelta, float maxforce, float axismaxdeltatrq) {

        if(isRealRobot) {
            iis_robot_dep::CartesianImpedance imp;

            imp.stiffness.linear.x = imp.stiffness.linear.y = imp.stiffness.linear.z = cpstiffnessxyz;
            imp.damping.linear.x = imp.damping.linear.y = imp.damping.linear.z = cpdamping;
            imp.stiffness.angular.x = imp.stiffness.angular.y = imp.stiffness.angular.z = cpstiffnessabc;
            imp.damping.angular.x = imp.damping.angular.y = imp.damping.angular.z = cpdamping;
            imp.cpmaxdelta = cpmaxdelta;
            imp.axismaxdeltatrq = axismaxdeltatrq;

            iis_robot_dep::FriJointImpedance newImpedance;
            for (int j = 0; j < 7; j++){
                newImpedance.stiffness[j] = cpstiffnessxyz;
                newImpedance.damping[j] = cpdamping;
            }

            pub_set_cart_stiffness.publish(imp);
            pub_set_joint_stiffness.publish(newImpedance);
            ros::spinOnce();
        } else {
            if(!isShutUp())
                ROS_INFO("(setStiffness) this functionality is not available in simulator - ignored");
        }

    }
Exemplo n.º 3
0
    void KukieControlQueue::setAdditionalLoad(float loadMass, float loadPos) {

        if(isRealRobot) {

            std_msgs::Float32MultiArray msg;
            msg.layout.dim.push_back(std_msgs::MultiArrayDimension());
            msg.layout.dim[0].size = 2;
            msg.layout.dim[0].stride = 0;
            msg.layout.dim[0].label = "Load";
            msg.data.push_back(loadMass);
            msg.data.push_back(loadPos);

            pubAddLoad.publish(msg);

            int tmpMode = getCurrentControlType();
            stopCurrentMode();
            switchMode(tmpMode);

        } else {
            if(!isShutUp())
                ROS_INFO("(setAdditionalLoad) this functionality is not available in simulator - ignored");
        }

    }
Exemplo n.º 4
0
    void ControlQueue::run() {

        setInitValuesInternal();
        ros::Rate sleepRate(1.0 / sleepTime);

        arma::vec movement = arma::vec(1);
        geometry_msgs::Pose movementPose;

        if(getStartingJoints().n_elem > 1) {

            if(!isShutUp())
                ROS_INFO("(ControlQueue) start moving to start position");
            jointPtp(getStartingJoints());

            if(!isShutUp())
                ROS_INFO("(ControlQueue) finished moving to start position");

        }

        isInit = true;

        lastDuration = 0.0;
        double toleratedMaxDuration = 1.1 * getCycleTime();
        double toleratedMinDuration = 0.9 * getCycleTime();

        movement = getCurrentJoints().joints;

        t.tic("r");

        while(!finish && ros::ok) {

            t.tic("d");

            /*
            // switched off adaptive cycle time behaviour for now - probably requires a PID controller for that
            if(toleratedMinDuration < lastDuration || lastDuration > toleratedMaxDuration) {

                sleepTime = max(0.000000001, sleepTime + 0.3 * (desiredCycleTime -  lastDuration));
                sleepRate = ros::Rate(1.0 / sleepTime);

            }
            */

            currentTime = t.toc("r");

            currentJoints = getCurrentJoints().joints;
            currentCartPose = getCurrentCartesianPose();

            if(rollbackMode) {

                rollBackQueue.push_front(currentJoints);

                // if queue is full --> go back
                while(rollBackQueue.size() > rollBackQueueSize)
                    rollBackQueue.pop_back();

            }

            if(!stopQueueWhilePtp() || !jointPtpRunning && !cartesianPtpRunning) {

                if(currentControlType == CONTROLQUEUE_JNT_IMP_MODE || currentControlType == CONTROLQUEUE_JNT_POS_MODE) {

                    if(movementQueue.size() > 0) {

                        // move to position in queue
                        movement = movementQueue.front();
                        movementQueue.pop();

                    } else {

                        if(stopQueueWhilePtp())
                            movement = getCurrentJoints().joints;

                    }

                    submitNextJointMove(movement);

                } else if(currentControlType == CONTROLQUEUE_CART_IMP_MODE) {

                    if(cartesianMovementQueue.size() > 0) {

                        // move to position in queue
                        movementPose = cartesianMovementQueue.front();
                        cartesianMovementQueue.pop();

                    } else {

                        movementPose = getCurrentCartesianPose();

                    }

                    submitNextCartMove(movementPose);

                }

            }

            sleepRate.sleep();

            ros::spinOnce();

            lastDuration = t.toc("d");

        }

        if(!isShutUp())
            cout << "thread finished" << endl;

    }