iiwa LBR 7 R800: EndEffectorPose is upside down

[MightyMirko]

Hello and Thank you,

i tested your library today with a KUKA FRI Connection. It works ofc, but using the given model file from rl-examples i cant get the correct Transformation of the end effector Orientation:

**Details**

```c++ robotModel::robotModel(const std::string &xmlFilePath) { rl::mdl::XmlFactory factory; std::shared_ptr tempModel(factory.create(xmlFilePath)); model = std::move(tempModel); // Move ownership to the member variable kinematics = dynamic_cast(model.get()); q.resize(kinematics->getDof()); //std::printf("DoF: %i",sizeof(q)); } void robotModel::performForwardKinematics() { // q *= rl::math::DEG2RAD; kinematics->setPosition(q); kinematics->forwardPosition(); rl::math::Transform t = kinematics->getOperationalPosition(0); rl::math::Vector3 position = t.translation(); //std::cout << "Was steckt in T " <

Output is:

Session State:  MONITORING_READY
x: -0.45935 m   y: -0.00375237 m    z: 0.613881 m
a: -179.617 deg b: -28.2452 deg c: 179.657 deg
Joint configuration in degrees:    0.465831 -0.00258179  0.00238266     92.1643 -0.00076904    -59.5855    0.715265
End-effector position: [m]    -0.45935 -0.00375237    0.613881 orientation [deg] -179.617 -28.2452  179.657

Whereas a should be positive and c negative according to my sunrise smartpad > cartesian position in world frame on flange.

Do i have to change the rlmdl/kuka-lbr-iiwa-7-r800.xml or is this correct behaviour?

2:

Perhaps i oversaw it, but is there also a rlkin file for the 7 r800? EDIT: apparently kin is deprecated?

My Goal is to get TCP Position and Velocity. Also i want to write a simple endeffector mockup which is using this velocity.

[rickertm]

The KUKA ABC orientation values represent Euler angles in the convention z-y'-x'' calculated as R = R<sub>z</sub>(A) R<sub>y</sub>(B) R<sub>x</sub>(C). To calculate these values from a rotation matrix, you can use

rl::math::Vector3 orientation = t.rotation().eulerAngles(2, 1, 0);
rl::math::Real a = orientation(0);
rl::math::Real b = orientation(1);
rl::math::Real c = orientation(2);

I do not currently have a rl::kin model definition for the iiwa 7 r800. The kinematic model is easy to define, but the existing rl::sg geometric models use different coordinate systems and have to be adjusted—rl::mdl does not require rotation around the z axis, whereas rl::kin uses DH notation according to Paul. The development focuses on rl::mdl, as it can easily support different joint models. The earlier rl::kin is still kept for backward compatibility and as a DH reference implementation at the moment.

[MightyMirko]

Hi Markus,

thanks for the reply.

I have it running like you suggested.

void robotModel::performForwardKinematics() {
//    q *= rl::math::DEG2RAD;

    kinematics->setPosition(q);
    kinematics->forwardPosition();
    rl::math::Transform t = kinematics->getOperationalPosition(0);
    rl::math::Vector3 position = t.translation();
    //std::cout << "Was steckt in T " <<position<< std::endl;
    rl::math::Vector3 orientation = t.rotation().eulerAngles(2, 1, 0).reverse();
    rl::math::Real a = orientation(0) * rl::math::RAD2DEG;
    rl::math::Real b = orientation(1) * rl::math::RAD2DEG;
    rl::math::Real c = orientation(2) * rl::math::RAD2DEG;
    std::cout<<"a: "<<a<<"\tb: "<<b<<"\tc: "<<c<<std::endl;
    //printVector(position, orientation);
}

Output:

Session State:  MONITORING_READY
 J0: 0.465831
 J1: -0.002582
 J2: 0.002372
 J3: 92.164334
 J4: -0.000731
 J5: -59.585480
 J6: 0.715268
a: -179.617 b: -28.2452 c: 179.657
Session State:  MONITORING_READY

Sunrise Station:

[MightyMirko]

https://github.com/roboticslibrary/rl-examples/blob/master/rlmdl/kuka-lbr-iiwa-7-r800.xml

This is the xml im using for the model

[rickertm]

Please note the difference in the extra .reverse(), it should be

rl::math::Vector3 orientation = t.rotation().eulerAngles(2, 1, 0);

[MightyMirko]

Oh sorry for that :-)

Could you help me with a follow up question? What would be a good approach to get TCP Velocity? Do i have to go for Jacobian or is there already a built in solution?

[rickertm]

You can get the TCP velocity from your joint velocity via the forwardVelocity function

kinematics->setPosition(q);
kinematics->setVelocity(qd);
kinematics->forwardVelocity();
rl::math::MotionVector xd = kinematics->getOperationalVelocity(0);
// transform to world frame
kinematics->forwardPosition();
rl::math::Vector3 v = kinematics->getOperationalPosition(0).linear() * xd.linear();
rl::math::Vector3 omega = kinematics->getOperationalPosition(0).linear() * xd.angular();

or via the Jacobian matrix:

kinematics->setPosition(q);
kinematics->forwardPosition();
kinematics->calculateJacobian();
rl::math::Vector xd = kinematics->getJacobian() * qd;

[MightyMirko]

Hello Markus,

ok im trying it right now.

I do have one misunderstanding and perhaps overestimation in this repo, but with FRI i can only get Torques and Joint Positions. Each cycle (5ms) I'm setting the sent joint positions

// Update q from FRI
    lbr.q << jointPos[0],
            jointPos[1],
            jointPos[2],
            jointPos[3],
            jointPos[4],
            jointPos[5],
            jointPos[6];

In the proposed solution it is necessary to have the joint velocities qd? My question is whether there is a way to calculate joint velocities ($\dot q$​) directly from the FRI data, or if I need to derive them manually based on the successive joint positions. I would appreciate any guidance or clarification you can provide on this matter.

Thank you for your time and assistance.

[MightyMirko]

Ok got it now:

/*
 * One Sided differencing for numerical differentiation
 */
rl::math::Vector mastersclient::calcJointVel(double dt) {
    size_t size = jointTest.size();
    rl::math::Vector derivativeVector(size);
    derivativeVector.setZero();

    // Central differencing for numerical differentiation
    for (size_t i = 1; i < size - 1; ++i) {
        derivativeVector(i) = (jointTest[i] - oldJointPos[i]) / (dt);
        derivativeVector(i) = std::round(derivativeVector(i) * 10000.0) / 10000.0;
    }

    return derivativeVector;
}

void mastersclient::calcRobot() {
    try {
        getCurrentTimestamp();
        const double *measuredJointPosPtr = robotState().getMeasuredJointPosition();
        // KOpiere von vorne nach hinten in jointpos
        oldJointPos = jointTest;
        std::copy(measuredJointPosPtr, measuredJointPosPtr + LBRState::NUMBER_OF_JOINTS, jointTest.begin());

        // Calculate the derivative of the joint positions
        rl::math::Vector jointVel = calcJointVel(_stepWidth);  // Assuming dt is 5ms
        //std::cout << "Joint Velocities: " << jointVel << std::endl;
        robotmdl->setQ(jointTest, jointVel);
        robotmdl->performForwardKinematics();
        robotmdl->getTransformation();
        robotmdl->getTCPvelocity();

    } catch (const std::runtime_error &e) {
        printf("Not connected yet;\n");
    } catch (const std::exception &e) {
        std::cerr << "Exception caught: " << e.what() << std::endl;
    }
}

RobotModel.cpp

robotModel::robotModel(const std::string &xmlFilePath) {
    rl::mdl::XmlFactory factory;
    try {
        std::shared_ptr<rl::mdl::Model> tempModel(factory.create(xmlFilePath));
        model = std::move(tempModel);  // Move ownership to the member variable
        std::cout << "Model created successfully!" << std::endl;
    } catch (const std::exception &e) {
        std::cerr << "Exception caught: " << e.what() << std::endl;
        // Handle the exception as needed
    }

    kinematics = dynamic_cast<rl::mdl::Kinematic *>(model.get());
    //dynamic = dynamic_cast<rl::mdl::Dynamic *>(model.get());

    if (kinematics != nullptr) {//|| dynamic != nullptr) {
        // Dynamic cast was successful
        // You can use kinematics here
    } else {
        // Dynamic cast failed
        // Handle the failure or print an error message
        std::cerr << "Dynamic cast to rl::mdl::Kinematic or Dynamic failed." << std::endl;
    }

    lbr.q.resize(model->getDof());
    //std::printf("DoF: %i",sizeof(lbr_q));
    std::cout << "\n\n\n Using File: " << xmlFilePath << std::endl;

    lbr.q = model->getPosition();
    // lbr.qd = q.der
    lbr.qd = model->getVelocity();
    lbr.qdd = model->getAcceleration();
    lbr.tau = model->getTorque();
    //  this->printQ();

}

void robotModel::setQ(std::vector<double> &q, rl::math::Vector &qd) {
    // Setze akuelle FRI Werte in Kinematicberechnung
    assert(q.size() == lbr.q.size());
    assert(qd.size() == lbr.qd.size());
    lbr.q(q.data(), q.size());
    lbr.qd = qd;
}

void robotModel::performForwardKinematics() {
    kinematics->setPosition(this->lbr.q);
    kinematics->setVelocity(this->lbr.qd);

    kinematics->forwardPosition();
    kinematics->forwardVelocity();
}

void robotModel::getTransformation() {
    //auto xd = kinematics->getJacobian() * lbr.qd;
    rl::math::MotionVector xd = kinematics->getOperationalVelocity(0);
    kinematics->forwardPosition();
    tcp.vecV = kinematics->getOperationalPosition(0).linear() * xd.linear();
    tcp.vecOmega = kinematics->getOperationalPosition(0).linear() * xd.angular();
}

void robotModel::getTCPvelocity() {
    // Convert values to degrees
    tcp.vecV = rl::math::RAD2DEG * tcp.vecV;
    tcp.vecOmega = rl::math::RAD2DEG * tcp.vecOmega;
    // Print all values of vecV with labels
    std::cout << "Linear Velocity (vecV): [Vx, Vy, Vz] = [" << tcp.vecV.transpose() << "]" << std::endl;
    // Print all values of vecOmega with labels
    std::cout << "Angular Velocity (vecOmega): [Omega_x, Omega_y, Omega_z] = [" << tcp.vecOmega.transpose() << "]"
              << std::endl;
}

Linear Velocity (vecV): [Vx, Vy, Vz] = [12.9601       0       0]
Angular Velocity (vecOmega): [Omega_x, Omega_y, Omega_z] = [      0 13.9859       0]
Linear Velocity (vecV): [Vx, Vy, Vz] = [12.9448       0       0]
Angular Velocity (vecOmega): [Omega_x, Omega_y, Omega_z] = [      0 13.9916       0]
Linear Velocity (vecV): [Vx, Vy, Vz] = [13.032      0      0]
Angular Velocity (vecOmega): [Omega_x, Omega_y, Omega_z] = [      0 14.0833       0]

[rickertm]

If you need a more accurate value for the velocities and your trajectories are defined based on rl::math::Polynomial or rl::math::Spline, you can calculate the respective derivatives (velocities, accelerations, etc.) via the derivative() functions.

[MightyMirko]

Hi Markus,

its ok, i upgraded to 5point Stencil and it works fine imho.

I want to attach a endeffector and a workpiece. After gripping id like to calculate the workpiece velocity and position.

Is it necessary to create a second model of gripper/workpiece and attach it to the robot/gripper?

Is there an example demo which i should look into?