Ejemplo n.º 1
0
    void RosSchunk::moveJoints(arma::vec positions) {

        std::vector<double> stdPos = armadilloToStdVec(positions);
        std_msgs::Float64MultiArray newJoints;
        for(int i = 0; i < stdPos.size(); ++i) {
            if(stdPos.at(i) != SDH_IGNORE_JOINT)
                newJoints.data.push_back(stdPos.at(i));
            else
                newJoints.data.push_back(currentCommandedPos.at(i));
        }

        currentCommandedPos = newJoints.data;

        // get current position
        ros::spinOnce();

        targetReached = false;
        movementStarted = false;

        initCurrentPos = currentPos;
        trajPub.publish(newJoints);

        if(waitForReached) {

            while(!targetReached)
                ros::spinOnce();

        }

    }
Ejemplo n.º 2
0
bool CustomKinestheticTeacher::isDifferentialCommandSafe(arma::vec diffCommand,arma::vec currentJointStates){
    bool okay=true;
    int checkTimesAhead=3;
    for (auto i = 1;i<=checkTimesAhead;i++)
        if (isColliding(armadilloToStdVec(i*diffCommand + currentJointStates)))
            okay=false;
    return okay;
}
Ejemplo n.º 3
0
    void KukieControlQueue::computeCurrentCartPose() {

        if(!kinematicsInitialized) {

            kin = KUKADU_SHARED_PTR<Kinematics>(new KomoPlanner(shared_from_this(),
                                                                     resolvePath("$KUKADU_HOME/external/komo/share/data/kuka/data/iis_robot.kvg"),
                                                                     resolvePath("$KUKADU_HOME/external/komo/share/data/kuka/config/MT.cfg"),
                                                                     getRobotSidePrefix(), acceptCollisions));
            kinematicsInitialized = true;

        }

        while(getQueueRunning()) {

            forwadKinMutex.lock();
                currCarts = kin->computeFk(armadilloToStdVec(getCurrentJoints().joints));
            forwadKinMutex.unlock();

        }

    }
Ejemplo n.º 4
0
 std::vector<arma::vec> Kinematics::computeIk(arma::vec currentJointState, const geometry_msgs::Pose &goal) {
     return computeIk(armadilloToStdVec(currentJointState), goal);
 }
Ejemplo n.º 5
0
arma::vec CustomKinestheticTeacher::getNextDifferentialCommand(Eigen::MatrixXd jacobian,arma::vec currentJointState, ControllerType myType){


    std::vector<double> sensorReading= armadilloToStdVec(robotinoQueue->getCurrentProcessedCartesianFrcTrq().joints);
    auto numberOfCartesianFTs=jacobian.rows();
    if (numberOfCartesianFTs != sensorReading.size()){
        cout << "Problem in sensor readings vector size" << endl;
        return stdToArmadilloVec({0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0});
    }
    std::vector<double> forceVector = scaleForcesTorques(sensorReading);
    std::vector<double> additiveDifferential;

    switch(myType){
    case JACOBIAN:
    {

        additiveDifferential =  eigenToStdVec(jacobian.transpose() * stdToEigenVec(forceVector));
        additiveDifferential= scaleJointCommands(additiveDifferential);
        break;
    }
    case INVERSE://Pseudo inverse..horrible!
    {
        Eigen::JacobiSVD<Eigen::MatrixXd> svd(jacobian, Eigen::ComputeThinU | Eigen::ComputeThinV);
        additiveDifferential= scaleJointCommands(eigenToStdVec(0.15*svd.solve(stdToEigenVec(forceVector))));
        break;
    }
    case IK:
    {
        const double scaleIK=0.2;
        forceVector = armadilloToStdVec( scaleIK*stdToArmadilloVec(forceVector));
        geometry_msgs::Pose currentPose = mvKin->computeFk(armadilloToStdVec(currentJointState));
        geometry_msgs::Pose newPose;
        newPose.position.x= currentPose.position.x + forceVector.at(0);
        newPose.position.y=currentPose.position.y + forceVector.at(1);
        newPose.position.z=currentPose.position.z + forceVector.at(2);
        tf::Quaternion quat(currentPose.orientation.x,currentPose.orientation.y,currentPose.orientation.z,currentPose.orientation.w);
        tf::Matrix3x3 m(quat);
        double roll,pitch,yaw;
        m.getRPY(roll,pitch,yaw);
        roll+= forceVector.at(3);
        pitch+= forceVector.at(4);
        yaw+= forceVector.at(5);
        newPose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
        auto planIK = mvKin->computeIk(armadilloToStdVec(currentJointState),newPose);
        auto target =currentJointState;
        if (planIK.size()>0)
            target=planIK.back();
        else
            cout << "IK solution not found" << endl;

        additiveDifferential = armadilloToStdVec( target - currentJointState);
        if (isBigJump(additiveDifferential)){
            additiveDifferential={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
            cout << " Big Jump from IK solution" << endl;
        }
        break;
    }
    case HYBRID:
    {
        const double scaleIK=0.1;
        additiveDifferential={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
        forceVector = armadilloToStdVec( scaleIK*stdToArmadilloVec(forceVector));
        geometry_msgs::Pose currentPose = mvKin->computeFk(armadilloToStdVec(currentJointState));
        geometry_msgs::Pose newPose;
        newPose.position.x= currentPose.position.x + forceVector.at(0);
        newPose.position.y=currentPose.position.y + forceVector.at(1);
        newPose.position.z=currentPose.position.z + forceVector.at(2);
        tf::Quaternion quat(currentPose.orientation.x,currentPose.orientation.y,currentPose.orientation.z,currentPose.orientation.w);
        tf::Matrix3x3 m(quat);
        double roll,pitch,yaw;
        m.getRPY(roll,pitch,yaw);
        roll+= forceVector.at(3);
        pitch+= forceVector.at(4);
        yaw+= forceVector.at(5);
        newPose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
        auto planIK = mvKin->computeIk(armadilloToStdVec(currentJointState),newPose);
        auto target =currentJointState;
        bool jacobianUse = false;
        if (planIK.size()>0){
            target=planIK.back();
            additiveDifferential = armadilloToStdVec( target - currentJointState);
        }
        else
            jacobianUse =true;

        if (isBigJump(additiveDifferential))
            jacobianUse =true;


        if (jacobianUse){
            additiveDifferential =  eigenToStdVec(jacobian.transpose() * stdToEigenVec(forceVector));
            additiveDifferential= scaleJointCommands(additiveDifferential);
            cout << " jacobian" << endl;
        }
        break;
    }
    }


    return stdToArmadilloVec(capVec(additiveDifferential,MAXIMUM_JOINT_STEP));

}