基于RobWork的机器人运动学DH模型构建和求解

CMakeLists.txt

cmake_minimum_required(VERSION 2.8)
set(CMAKE_CXX_STANDARD 14)
project(robwork_demo)
#set(CMAKE_MODULE_PATH "")
set(RobWork_DIR "/home/liuqiang/Documents/RobWork-master/RobWork/RobWork/cmake")
find_package(RobWork)
find_package(Eigen3 REQUIRED)
include_directories(${ROBWORK_INCLUDE_DIRS})
message(STATUS "ROBWORK_INCLUDE_DIRS: "${ROBWORK_INCLUDE_DIRS})
link_directories(/home/liuqiang/Documents/RobWork-master/RobWork/RobWork/libs/relwithdebinfo)
add_executable(${PROJECT_NAME} main2.cpp)
message(STATUS "ROBWORK_LIBRARIES: " ${ROBWORK_LIBRARIES})
target_link_libraries(${PROJECT_NAME} ${ROBWORK_LIBRARIES})

main.cpp

#include <rw/common.hpp>
#include <rw/math.hpp>
#include <rw/kinematics.hpp>
#include <rw/models.hpp>

#include <rw/kinematics/FixedFrame.hpp>
#include <rw/models/SerialDevice.hpp>
#include <rw/models/RevoluteJoint.hpp>
#include <rw/kinematics/Kinematics.hpp>

#include <iostream>
using namespace rw::math;
using namespace rw::models;
using namespace rw::kinematics;
using namespace rw::common;

SerialDevice::Ptr getUR3Robot(StateStructure &state_structure) {
    Frame::Ptr base = ownedPtr(new FixedFrame("base", Transform3D<>::identity()));
    //Transform3D<>::DH();
    Joint::Ptr joint0 = ownedPtr(new RevoluteJoint("joint0", Transform3D<>::identity()));
//    Joint::Ptr joint1 = ownedPtr(new RevoluteJoint("joint1", Transform3D<>::DH(-Pi / 2, 0, 0.140, 0)));
//    Joint::Ptr joint2 = ownedPtr(new RevoluteJoint("joint2", Transform3D<>::DH(0, 0.270, 0, -Pi / 2)));
//    Joint::Ptr joint3 = ownedPtr(new RevoluteJoint("joint3", Transform3D<>::DH(0, 0.256, 0, 0)));
//    Joint::Ptr joint4 = ownedPtr(new RevoluteJoint("joint4", Transform3D<>::DH(Pi / 2, 0, 0.1035, Pi / 2)));
//    Joint::Ptr joint5 = ownedPtr(new RevoluteJoint("joint5", Transform3D<>::DH(-Pi / 2, 0, 0.0980, -Pi / 2)));
//    Frame::Ptr tcp = ownedPtr(new FixedFrame("tcp", Transform3D<>(Vector3D<>(0, 0, 0.0890))));
//    Joint::Ptr joint1 = ownedPtr(new RevoluteJoint("joint1", Transform3D<>::DH(M_PI / 2 + -0.000626175392884231741, 1.85427703966640276e-05, 0.15190 + -5.56630992100959343e-05, -5.05343367106411137e-05)));
//    Joint::Ptr joint2 = ownedPtr(new RevoluteJoint("joint2", Transform3D<>::DH(-0.00167062908967277168, -0.24365 + 0.000418280909411400392, 8.16504944596539062, -0.0561242156763783057)));
//    Joint::Ptr joint3 = ownedPtr(new RevoluteJoint("joint3", Transform3D<>::DH(0.00403522262881641277, -0.21325 + 0.001565903694771692, -14.4282499666997612, -0.0628157016125706208)));
//    Joint::Ptr joint4 = ownedPtr(new RevoluteJoint("joint4", Transform3D<>::DH(M_PI / 2 - 0.000368945620575544808, -9.66540899511680791e-06, 0.11235 + 6.26317959859598705, 0.118814438034714087)));
//    Joint::Ptr joint5 = ownedPtr(new RevoluteJoint("joint5", Transform3D<>::DH(-M_PI / 2 + 0.000173987342825920877, -2.93410449126249217e-05, 0.08535 + -3.9946104585394937e-05, 5.68822754944051158e-05)));
//    Frame::Ptr tcp = ownedPtr(new FixedFrame("tcp", Transform3D<>::DH(0.0, 0.0, 0.08190 + 0.000367523159254548593, -1.45429512938645578e-05)));
    Joint::Ptr joint1 = ownedPtr(new RevoluteJoint("joint1", Transform3D<>::DH(89.9641227927528 * Deg2Rad, 1.85427703966640276e-05, 0.15184433690079, -0.00289540421401276 * Deg2Rad)));
    Joint::Ptr joint2 = ownedPtr(new RevoluteJoint("joint2", Transform3D<>::DH(-0.0957199959700326 * Deg2Rad, -0.243231719090589, 8.16504944596539062, -3.21568068673845 * Deg2Rad)));
    Joint::Ptr joint3 = ownedPtr(new RevoluteJoint("joint3", Transform3D<>::DH(0.231201226026866 * Deg2Rad, -0.211684096305228, -14.4282499666997612, -3.59907458955342 * Deg2Rad)));
    Joint::Ptr joint4 = ownedPtr(new RevoluteJoint("joint4", Transform3D<>::DH(89.9788609730712 * Deg2Rad, -9.66540899511681e-06, 6.37552959859599, 6.80756584460776 * Deg2Rad)));
    Joint::Ptr joint5 = ownedPtr(new RevoluteJoint("joint5", Transform3D<>::DH(-89.9900312595674 * Deg2Rad, -2.93410449126249217e-05, 0.0853100538954146, 0.00325911431492984 * Deg2Rad)));
    Frame::Ptr tcp = ownedPtr(new FixedFrame("tcp", Transform3D<>::DH(0.0, 0.0, 0.08190 + 0.000367523159254548593, -1.45429512938645578e-05)));
//    Joint::Ptr joint1 = ownedPtr(new RevoluteJoint("joint1",
//                                 Transform3D<>(Vector3D<>(1.85428e-05, -9.37047e-10, 0.151844),
//                                         RPY<>(-5.05343e-05, 0, 1.57017))));
//    Joint::Ptr joint2 = ownedPtr(new RevoluteJoint("joint2",
//                                 Transform3D<>(Vector3D<>(-0.242849, 0.013644, 8.16505),
//                                         RPY<>( -0.0561242, 0, -0.00167063))));
//    Joint::Ptr joint3 = ownedPtr(new RevoluteJoint("joint3",
//                                 Transform3D<>(Vector3D<>(-0.211267, 0.0132883, -14.4282),
//                                         RPY<>(-0.0628157, 0, 0.00403522))));
//    Joint::Ptr joint4 = ownedPtr(new RevoluteJoint("joint4",
//                                 Transform3D<>(Vector3D<>(-9.59727e-06, -1.14569e-06, 6.37553),
//                                         RPY<>(0.118814, 0, 1.57043))));
//    Joint::Ptr joint5 = ownedPtr(new RevoluteJoint("joint5",
//                                 Transform3D<>(Vector3D<>(-2.9341e-05, -1.66899e-09,   0.0853101),
//                                         RPY<>(5.68823e-05, 0, -1.57062))));
//    Frame::Ptr tcp = ownedPtr(new FixedFrame("tcp", Transform3D<>(Vector3D<>(0, 0, 0.0822675), RPY<>(-1.4543e-05, 0, 0))));
    state_structure.addFrame(base);
    state_structure.addFrame(joint0, base);
    state_structure.addFrame(joint1, joint0);
    state_structure.addFrame(joint2, joint1);
    state_structure.addFrame(joint3, joint2);
    state_structure.addFrame(joint4, joint3);
    state_structure.addFrame(joint5, joint4);
    state_structure.addFrame(tcp, joint5);
    State state = state_structure.getDefaultState();
    const SerialDevice::Ptr device = ownedPtr(new SerialDevice(base.get(), tcp.get(), "ur3", state));
    std::pair<Q, Q> bounds;
    bounds.first = Q(6, -Pi, -Pi, -Pi, -Pi, -Pi, -Pi);
    bounds.second = Q(6, Pi, Pi, Pi, Pi, Pi, Pi);
    device->setBounds(bounds);
    return device;
}
int main() {
//    EAA<> axis_angle(Vector3D<>(1.0, 0, 0), M_PI / 4);
//    EAA<> axis_angle2(Vector3D<>(1.0, 0, 0), -M_PI / 4);
//    std::cout << axis_angle << "\n";
//    std::cout << axis_angle.toRotation3D() << "\n";
//    std::cout << axis_angle2 << "\n";
//    std::cout << axis_angle2.toRotation3D() << "\n";
//    std::cout << EAA<>(axis_angle2.toRotation3D()) << "\n";
    StateStructure stateStructure;
    const SerialDevice::Ptr device = getUR3Robot(stateStructure);
    State state = stateStructure.getDefaultState();
    std::cout << device->getDOF() << "\n";
    Q q(6, 0, 0, 0, 0, 0, 0);
    device->setQ(q, state);
    auto frames = device->frames();
    std::cout << frames.size() << "\n";
//    std::cout << Kinematics::frameTframe(frames.front(), frames.back(), state);
    for (auto frame : frames) {
        Transform3D<> trans = Kinematics::frameTframe(frame, frames.front(), state);
        std::cout << "Position: " << trans.P() << "\n";
        RPY<> rpy(trans.R());
        std::cout << "Attitude: " << rpy[0]*Rad2Deg << " " << rpy[1]*Rad2Deg << " " << rpy[2]*Rad2Deg << "\n";
        std::cout << "=====================================\n";
    }
//    Transform3D<> tcp_pose = device->baseTend(state);
//    std::cout << tcp_pose.P() << "\n";
//    std::cout << RPY<>(tcp_pose.R()) << "\n";
    return 0;
}