コード例 #1
0
ファイル: controlqueue.cpp プロジェクト: ipa-nhg/kukadu
    void ControlQueue::rollBack(double time) {

        rollbackMode = false;
        int rollBackCount = (int) (time / getCycleTime());

        int stretchFactor = ceil((double) rollBackCount / (double) rollBackQueue.size());
        stretchFactor = max((double) stretchFactor, 1.0);

        vec lastCommand(getMovementDegreesOfFreedom());
        if(rollBackQueue.size())
            lastCommand = rollBackQueue.front();

        int newRollBackCount = ceil((double) rollBackCount / (double) stretchFactor);

        // fill command queue with last commands (backwards)
        for(int i = 0; i < newRollBackCount && rollBackQueue.size(); ++i) {

            vec nextCommand = rollBackQueue.front();

            // interpolate to stretch the trajectory in case there are not enough measured packets (happens in usage with simulator)
            vec diffUnit = (nextCommand - lastCommand) / (double) stretchFactor;
            for(int j = 0; j < stretchFactor; ++j) {
                move(lastCommand + j * diffUnit);
            }

            lastCommand = nextCommand;
            rollBackQueue.pop_front();

        }

        // wait until everything has been executed
        synchronizeToQueue(1);
        rollBackQueue.clear();

    }
コード例 #2
0
ファイル: kukiecontrolqueue.cpp プロジェクト: shangl/iros2016
    void KukieControlQueue::submitNextJointMove(arma::vec joints) {

        std_msgs::Float64MultiArray nextCommand;
        for(int i = 0; i < getMovementDegreesOfFreedom(); ++i)
            nextCommand.data.push_back(joints[i]);
        pubCommand.publish(nextCommand);

    }
コード例 #3
0
ファイル: kukiecontrolqueue.cpp プロジェクト: shangl/iros2016
    void KukieControlQueue::jntFrcTrqCallback(const std_msgs::Float64MultiArray& msg) {

        if(isRealRobot)
            currentJntFrqTrq = stdToArmadilloVec(msg.data);
        else {
            currentJntFrqTrq = vec(getMovementDegreesOfFreedom());
            currentJntFrqTrq.fill(0.0);
        }

    }
コード例 #4
0
    mes_result PlottingControlQueue::getCurrentJntFrcTrq() {

        mes_result ret;
        vec frcTrq(getMovementDegreesOfFreedom());
        frcTrq.fill(0.0);

        ret.joints = frcTrq;
        ret.time = getCurrentTime();

        return ret;

    }
コード例 #5
0
ファイル: kukiecontrolqueue.cpp プロジェクト: shangl/iros2016
    void KukieControlQueue::constructQueue(double sleepTime, std::string commandTopic, std::string retPosTopic, std::string switchModeTopic, std::string retCartPosTopic,
                        std::string cartStiffnessTopic, std::string jntStiffnessTopic, std::string ptpTopic,
                        std::string commandStateTopic, std::string ptpReachedTopic, std::string addLoadTopic, std::string jntFrcTrqTopic, std::string cartFrcTrqTopic,
                        std::string cartPtpTopic, std::string cartPtpReachedTopic, std::string cartMoveRfQueueTopic, std::string cartMoveWfQueueTopic, std::string cartPoseRfTopic, std::string jntSetPtpThreshTopic, bool acceptCollisions, ros::NodeHandle node
                    ) {

        set_ctrlc_exit_handler();

        this->acceptCollisions = acceptCollisions;

        this->ptpTopic = ptpTopic;
        this->addLoadTopic = addLoadTopic;
        this->commandTopic = commandTopic;
        this->cartPtpTopic = cartPtpTopic;
        this->retJointPosTopic = retPosTopic;
        this->jntFrcTrqTopic = jntFrcTrqTopic;
        this->switchModeTopic = switchModeTopic;
        this->retCartPosTopic = retCartPosTopic;
        this->ptpReachedTopic = ptpReachedTopic;
        this->cartFrcTrqTopic = cartFrcTrqTopic;
        this->stiffnessTopic = cartStiffnessTopic;
        this->jntStiffnessTopic = jntStiffnessTopic;
        this->commandStateTopic = commandStateTopic;
        this->cartPtpReachedTopic = cartPtpReachedTopic;
        this->cartMoveRfQueueTopic = cartMoveRfQueueTopic;
        this->cartMoveWfQueueTopic = cartMoveWfQueueTopic;
        this->jntSetPtpThreshTopic = jntSetPtpThreshTopic;

        loop_rate = new ros::Rate(1.0 / sleepTime);

        cartesianPtpReached = 0;

        setInitValues();
        this->node = node;

        if(sleepTime == 0.0) {
            ROS_ERROR("(KukieControlQueue) the sleep time you provided is 0. note that it is required in seconds");
            throw KukaduException("(KukieControlQueue) the sleep time you provided is 0. note that it is required in seconds");
        }

        subJntPos = node.subscribe(retPosTopic, 2, &KukieControlQueue::robotJointPosCallback, this);
        subComState = node.subscribe(commandStateTopic, 2, &KukieControlQueue::commandStateCallback, this);
        subPtpReached = node.subscribe(ptpReachedTopic, 2, &KukieControlQueue::ptpReachedCallback, this);
        subjntFrcTrq = node.subscribe(jntFrcTrqTopic, 2, &KukieControlQueue::jntFrcTrqCallback, this);
        subCartFrqTrq = node.subscribe(cartFrcTrqTopic, 2, &KukieControlQueue::cartFrcTrqCallback, this);
        subCartPtpReached = node.subscribe(cartPtpReachedTopic, 2, &KukieControlQueue::cartPtpReachedCallback, this);
        subCartPoseRf = node.subscribe(cartPoseRfTopic, 2, &KukieControlQueue::cartPosRfCallback, this);

        pub_set_cart_stiffness = node.advertise<iis_robot_dep::CartesianImpedance>(stiffnessTopic, 1);
        pub_set_joint_stiffness = node.advertise<iis_robot_dep::FriJointImpedance>(jntStiffnessTopic, 1);
        pubCartPtp = node.advertise<geometry_msgs::Pose>(cartPtpTopic, 1);
        pubCartMoveRfQueue = node.advertise<geometry_msgs::Pose>(cartMoveRfQueueTopic, 1);
        pubCartMoveWfQueue = node.advertise<geometry_msgs::Pose>(cartMoveWfQueueTopic, 1);
        pub_set_ptp_thresh = node.advertise<std_msgs::Float64>(jntSetPtpThreshTopic, 1);

        pubCommand = node.advertise<std_msgs::Float64MultiArray>(commandTopic, 10);
        pubSwitchMode = node.advertise<std_msgs::Int32>(switchModeTopic, 1);
        pubPtp = node.advertise<std_msgs::Float64MultiArray>(ptpTopic, 10);

        isRealRobot = (getRobotDeviceType().compare("real")) ? false : true;

        // this is required because shared_from_this can't be called in constructor (initializiation happens by lazy loading)
        plannerInitialized = false;
        kinematicsInitialized = false;

        ros::Rate r(10);
        while(!firstJointsReceived || !firstModeReceived)
            r.sleep();

        currentControlType = impMode;

        currentCartFrqTrq = vec(6);
        currentCartFrqTrq.fill(0.0);

        currentJntFrqTrq = vec(getMovementDegreesOfFreedom());
        currentJntFrqTrq.fill(0.0);

        usleep(1e6);

    }