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;
}