From 8e25bb09a353d9ff474a054c3da8e1c9c4db8b97 Mon Sep 17 00:00:00 2001 From: Derek Witcpalek Date: Tue, 7 Apr 2020 22:45:22 -0400 Subject: [PATCH] Implemented initial dynamics model which is read in from an sdf file. There are currently some issues with coordinate transforms. --- rrrobot_src/src/arm_control/CMakeLists.txt | 233 +++++++++++++ .../src/arm_control/include/arm_control/arm.h | 73 ++++ rrrobot_src/src/arm_control/package.xml | 65 ++++ rrrobot_src/src/arm_control/src/arm.cpp | 313 ++++++++++++++++++ .../src/arm_control/src/arm_controller.cpp | 79 +++++ rrrobot_src/src/arm_control/test/test.cpp | 61 ++++ rrrobot_src/src/simulation_env/CMakeLists.txt | 5 +- 7 files changed, 827 insertions(+), 2 deletions(-) create mode 100644 rrrobot_src/src/arm_control/CMakeLists.txt create mode 100644 rrrobot_src/src/arm_control/include/arm_control/arm.h create mode 100644 rrrobot_src/src/arm_control/package.xml create mode 100644 rrrobot_src/src/arm_control/src/arm.cpp create mode 100644 rrrobot_src/src/arm_control/src/arm_controller.cpp create mode 100644 rrrobot_src/src/arm_control/test/test.cpp diff --git a/rrrobot_src/src/arm_control/CMakeLists.txt b/rrrobot_src/src/arm_control/CMakeLists.txt new file mode 100644 index 0000000..cf8517a --- /dev/null +++ b/rrrobot_src/src/arm_control/CMakeLists.txt @@ -0,0 +1,233 @@ +cmake_minimum_required(VERSION 2.8.3) +project(arm_control) + +## Compile as C++11, supported in ROS Kinetic and newer +add_compile_options(-std=c++11;-g3) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp + std_msgs +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) +find_package(orocos_kdl) +find_package(kdl_parser) + +# sdfconfig +find_package(PkgConfig REQUIRED) +pkg_check_modules(SDF sdformat REQUIRED) + +find_package(ignition-math6 REQUIRED) + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include ../simulation_env/include +# LIBRARIES arm_control +# CATKIN_DEPENDS roscpp std_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include/arm_control + ${catkin_INCLUDE_DIRS} + ${roscpp_INCLUDE_DIRS} + /usr/include/bullet + /usr/include/sdformat-6.2 + /usr/include/ignition/math6 + /home/rrrobot/rrrobot_src/devel/include/ +) +include_directories( + /usr/include/eigen3 +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/arm_control.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +add_executable(${PROJECT_NAME}_node src/arm_controller.cpp src/arm.cpp) +add_executable(test test/test.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +target_link_libraries(${PROJECT_NAME}_node + ${catkin_LIBRARIES} + ${kdl_parser_LIBRARIES} + ${orocos_kdl_LIBRARIES} + ${SDF_LIBRARIES} + ignition-math6::ignition-math6 +) + +target_link_libraries(test + ${catkin_LIBRARIES} + ${kdl_parser_LIBRARIES} + ${orocos_kdl_LIBRARIES} + ${SDF_LIBRARIES} + ignition-math6::ignition-math6 +) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +install(TARGETS ${PROJECT_NAME}_node + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_arm_control.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/rrrobot_src/src/arm_control/include/arm_control/arm.h b/rrrobot_src/src/arm_control/include/arm_control/arm.h new file mode 100644 index 0000000..b01d77e --- /dev/null +++ b/rrrobot_src/src/arm_control/include/arm_control/arm.h @@ -0,0 +1,73 @@ +#include +#include +#include +#include +#include +#include +#include + +#include + +class Arm +{ +public: + /* + * sdf_file: this is the sdf file that defines the model. This + * class will use that description of the robot to model + * its dynamics. + */ + Arm(const std::string &sdf_file); + + /* + * Gets the center of mass of the remaining arm, starting from the + * link in index (ie the center of mass of links l[i]-l[n]) + */ + KDL::Vector getCOM(size_t index); + /* + * Gets the center of mass of the remaining arm, starting from the + * link named 'link_name' + */ + KDL::Vector getCOM(const std::string &link_name); + + /* + * Gets the inertia of the remaining arm about the joint in index. This + * treats the preceding segments as if they are stationary and calculates + * the inertia of the remaining arm. + */ + float getInertia(size_t index); + /* + * Gets the inertia of the remaining arm about the joint named 'joint_name'. This + * treats the preceding segments as if they are stationary and calculates + * the inertia of the remaining arm. + */ + float getInertia(const std::string &joint_name); + + /* + * This returns the mass of the remaining links, starting from (and including) + * the link at index + */ + float getSupportedMass(size_t index); + /* + * This returns the mass of the remaining links, starting from (and including) + * the link named 'link_name' + */ + float getSupportedMass(const std::string &link_name); + + /* + * This returns the torques required to get the arm moving with the desired + * angular acceleration. + */ + std::vector getRequiredTorques(/*std::vector theta_des, std::vector theta_dot_des, */ std::vector theta_double_dot_des); + + const KDL::Chain &getArm() const; + +private: + KDL::Chain arm; + + /* + * i_com: the inertia about the center of mass of this link + * mass: the mass of this link + * distance: the distance from the center of mass to the new axis + */ + float parallelAxisTheorem(float i_com, float mass, float distance); +}; \ No newline at end of file diff --git a/rrrobot_src/src/arm_control/package.xml b/rrrobot_src/src/arm_control/package.xml new file mode 100644 index 0000000..c11d1c4 --- /dev/null +++ b/rrrobot_src/src/arm_control/package.xml @@ -0,0 +1,65 @@ + + + arm_control + 0.0.0 + The arm_control package + + + + + dwitcpa + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + std_msgs + roscpp + std_msgs + roscpp + std_msgs + + + + + + + + diff --git a/rrrobot_src/src/arm_control/src/arm.cpp b/rrrobot_src/src/arm_control/src/arm.cpp new file mode 100644 index 0000000..dc1e726 --- /dev/null +++ b/rrrobot_src/src/arm_control/src/arm.cpp @@ -0,0 +1,313 @@ +#include + +#include +#include +#include + +#include +#include +#include + +using std::cout; +using std::endl; +using std::string; +using std::stringstream; +using std::unordered_map; +using std::vector; + +struct FrameInformation +{ + string link_name; + KDF::Joint joint; + KDF::Frame link_frame; + KDF::Frame joint_frame; + RigidBodyInertia inertia; +}; + +/* + * sdf_file: this is the sdf file that defines the model. This + * class will use that description of the robot to model + * its dynamics. + */ +Arm::Arm(const std::string &sdf_file) +{ + // read sdf file + sdf::SDFPtr sdf(new sdf::SDF()); + sdf::init(sdf); + sdf::readFile(sdf_file, sdf); + + // get model + const sdf::ElementPtr root = sdf->Root(); + const sdf::ElementPtr model = root->GetElement("model"); + const string model_name = model->Get("name"); + cout << "Found " << model_name << " model" << endl; + + unordered_map links; + unordered_map link_ordering; + string first_link; + + // start reading links and joints + sdf::ElementPtr link = model->GetElement("link"); + while (link) + { + cout << "Link: " << link->Get("name") << endl; + + const string name(link->Get("name")); + KDL::Joint cur_joint(KDL::Joint::None); + + const sdf::ElementPtr inertial_data = link->GetElement("inertial"); + float mass = inertial_data->Get("mass"); + cout << "Mass: " << mass << endl; + + string inertial_data_s = inertial_data->GetElement("pose")->GetValue()->GetAsString(); + stringstream inertial_data_ss(inertial_data_s); + KDL::Vector inertial_frame_info; + + inertial_data_ss >> inertial_frame_info.data[0]; + inertial_data_ss >> inertial_frame_info.data[1]; + inertial_data_ss >> inertial_frame_info.data[2]; + + const sdf::ElementPtr inertia = inertial_data->GetElement("inertia"); + KDL::RotationalInertia rotational_inertia( + inertia->Get("ixx"), + inertia->Get("iyy"), + inertia->Get("izz"), + inertia->Get("ixy"), + inertia->Get("ixz"), + inertia->Get("iyz")); + + KDL::RigidBodyInertia link_inertia(mass, inertial_frame_info, rotational_inertia); + + // Transformation from world to link coordinates + // stringstream frame_location(link->GetElement("pose")->GetValue()->GetAsString()); + // float x, y, z; + // float roll, pitch, yaw; + // frame_location >> x >> y >> z >> roll >> pitch >> yaw; + // KDL::Rotation rotation = KDL::Rotation::RPY(roll, pitch, yaw); + // KDL::Vector link_position(x, y, z); + // KDL::Frame frame(rotation, link_position); + + KDL::Segment cur_link(name, cur_joint, KDL::Frame::Identity(), link_inertia); + //arm.addSegment(cur_link); + links[name] = cur_link; + + // cout << inertia->Get("ixx") << endl; + // cout << inertia->Get("iyy") << endl; + // cout << inertia->Get("izz") << endl; + // cout << inertia->Get("ixy") << endl; + // cout << inertia->Get("ixz") << endl; + // cout << inertia->Get("iyz") << endl; + + // inertial_data->GetElement("pose")->PrintValues(""); //PrintValues(""); + // cout << inertial_data->GetElement("pose")->GetAttributeCount() << endl; + // const sdf::ParamPtr pose = inertial_data->GetElement("pose")->GetValue(); + // const sdf::ElementPtr pose_elt = inertial_data->GetElement("pose"); + // cout << pose->GetAsString() << endl; + // // cout << pose->IsType() << endl; + // cout << pose << endl; + // cout << *pose_elt->GetValue() << endl; + // sdf::ElementPtr cur = pose_elt->GetFirstElement(); + // while (cur) + // { + // cout << cur->Get() << endl; + + // cur = cur->GetNextElement(); + // } + + //ignition::math::Pose3 inertial_pose = inertial_data->Get("pose"); + //cout << "Inertial pose: " << inertial_pose << endl; //inertial_pose.pos.x << '\t' << inertial_pose.pos.y << '\t' << << endl; + // float data; + // while (data << inertial_pose) + // { + // cout << data << '\t'; + // } + // cout << endl; + // KDL::RigidBodyInertia( + // inertial_data->Get("mass"), + + // ); + // KDL::Segment + // current_link() + // arm.addSegment() + + link = link->GetNextElement("link"); + } + + cout << "# of arm segments: " << arm.getNrOfSegments() << "\t# of arm joints: " << arm.getNrOfJoints() << endl; + + sdf::ElementPtr joint = model->GetElement("joint"); + while (joint) // TODO(dwitcpa): add support for translational joints + { + const string name(joint->Get("name")); + const string parent(joint->GetElement("parent")->GetValue()->GetAsString()); + const string child(joint->GetElement("child")->GetValue()->GetAsString()); + link_ordering[parent] = child; + if (parent == string("world")) + { + joint = joint->GetNextElement("joint"); + continue; //first_link = child; + } + + stringstream axis(joint->GetElement("axis")->GetElement("xyz")->GetValue()->GetAsString()); + + int axis_num = 0; + int cur; + while (axis >> cur) + { + if (cur != 0) + break; + axis_num++; + } + KDL::Joint::JointType joint_type = KDL::Joint::JointType::RotAxis; + if (axis_num == 0) + joint_type = KDL::Joint::JointType::RotX; + else if (axis_num == 1) + joint_type = KDL::Joint::JointType::RotY; + else if (axis_num == 2) + joint_type = KDL::Joint::JointType::RotZ; + + KDL::Joint cur_joint(name, joint_type); + + stringstream pose_string(joint->GetElement("pose")->GetValue()->GetAsString()); + float x, y, z; + float roll, pitch, yaw; + pose_string >> x >> y >> z >> roll >> pitch >> yaw; + + KDL::Rotation frame_rotation = KDL::Rotation::RPY(roll, pitch, yaw); + KDL::Vector frame_location(x, y, z); + + links[child] = KDL::Segment(links[child].getName(), cur_joint, KDL::Frame(frame_rotation, frame_location), links[child].getInertia()); + //KDL::Segment cur_segment = arm.getSegment + + joint = joint->GetNextElement("joint"); + } + + string cur_link_name = "world"; + KDL::Frame prev_frame(KDL::Rotation::RPY(0, 0, 0), KDL::Vector(0, 0, 0)); + // btTransform prev_frame_world_transform(btMatrix3x3::getIdentity()); + while (link_ordering.find(cur_link_name) != link_ordering.end()) + { + cur_link_name = link_ordering[cur_link_name]; + + const KDL::Segment &cur = links[cur_link_name]; + const KDL::Frame &cur_frame = cur.getFrameToTip(); + double cur_roll, cur_pitch, cur_yaw; + cur_frame.M.GetRPY(cur_roll, cur_pitch, cur_yaw); + double prev_roll, prev_pitch, prev_yaw; + prev_frame.M.GetRPY(prev_roll, prev_pitch, prev_yaw); + KDL::Rotation new_rotation = KDL::Rotation::RPY(cur_roll - prev_roll, cur_pitch - prev_pitch, cur_yaw - prev_yaw); + double roll, pitch, yaw; + new_rotation.GetRPY(roll, pitch, yaw); + + KDL::Vector cur_pos = cur_frame.p; + KDL::Vector prev_pos = prev_frame.p; + KDL::Vector new_position(cur_pos.x() /* - prev_pos.x()*/, cur_pos.y() /* - prev_pos.y()*/, cur_pos.z() /* - prev_pos.z()*/); + + // const KDL::Rotation &cur_rotation = cur_frame.M; + // const KDL::Vector &cur_translation = cur_frame.p; + // const KDL::Vector &rot_x = cur_rotation.UnitX(); + // const KDL::Vector &rot_y = cur_rotation.UnitY(); + // const KDL::Vector &rot_z = cur_rotation.UnitZ(); + + // btTransform cur_frame_world_transform( + // btMatrix3x3(rot_x[0], rot_y[0], rot_z[0], + // rot_x[1], rot_y[1], rot_z[1], + // rot_x[2], rot_y[2], rot_z[2]), + // btVector3(btScalar(cur_translation.x()), + // btScalar(cur_translation.y()), + // btScalar(cur_translation.z()))); + // btTransform transform; + // //transform.mult(prev_frame_world_transform, cur_frame_world_transform /*.inverse()*/); + // transform = cur_frame float roll, pitch, yaw; + // transform.getRotation().getEulerZYX(yaw, pitch, roll); + // KDL::Rotation rot(KDL::Rotation::RPY(roll, pitch, yaw)); + // btVector3 translation = transform.getOrigin(); + // KDL::Vector trans((float)translation.getX(), (float)translation.getY(), (float)translation.getZ()); + + cout << cur_link_name << endl; + // cout << "\tprev_frame world transform ("; + + cout << "\tOrigin position: " << new_position.x() << ", " << new_position.y() << ", " << new_position.z() << endl; + cout << "\tFrame rotation (roll, pitch, yaw): (" << roll << ", " << pitch << ", " << yaw << ")" << endl; + + KDL::Segment to_add(cur.getName(), cur.getJoint(), KDL::Frame(new_rotation, new_position), cur.getInertia()); + arm.addSegment(to_add); + + prev_frame = cur_frame; + // prev_frame_world_transform = cur_frame_world_transform; + } + + cout << arm.getNrOfJoints() << '\t' << arm.getNrOfSegments() << endl; +} + +/* + * Gets the center of mass of the remaining arm, starting from the + * link in index (ie the center of mass of links l[i]-l[n]) + */ +KDL::Vector Arm::getCOM(size_t index) +{ +} + +/* + * Gets the center of mass of the remaining arm, starting from the + * link named 'link_name' + */ +KDL::Vector Arm::getCOM(const std::string &link_name) +{ +} + +/* + * Gets the inertia of the remaining arm about the joint in index. This + * treats the preceding segments as if they are stationary and calculates + * the inertia of the remaining arm. + */ +float Arm::getInertia(size_t index) +{ +} + +/* + * Gets the inertia of the remaining arm about the joint named 'joint_name'. This + * treats the preceding segments as if they are stationary and calculates + * the inertia of the remaining arm. + */ +float Arm::getInertia(const std::string &joint_name) +{ +} + +/* + * This returns the mass of the remaining links, starting from (and including) + * the link at index + */ +float Arm::getSupportedMass(size_t index) +{ +} + +/* + * This returns the mass of the remaining links, starting from (and including) + * the link named 'link_name' + */ +float Arm::getSupportedMass(const std::string &link_name) +{ +} + +/* + * This returns the torques required to get the arm moving with the desired + * angular acceleration. + */ +std::vector Arm::getRequiredTorques(/*std::vector theta_des, std::vector theta_dot_des, */ std::vector theta_double_dot_des) +{ +} + +const KDL::Chain &Arm::getArm() const +{ + return arm; +} + +/* + * i_com: the inertia about the center of mass of this link + * mass: the mass of this link + * distance: the distance from the center of mass to the new axis + */ +float Arm::parallelAxisTheorem(float i_com, float mass, float distance) +{ +} \ No newline at end of file diff --git a/rrrobot_src/src/arm_control/src/arm_controller.cpp b/rrrobot_src/src/arm_control/src/arm_controller.cpp new file mode 100644 index 0000000..e285f83 --- /dev/null +++ b/rrrobot_src/src/arm_control/src/arm_controller.cpp @@ -0,0 +1,79 @@ +#include +#include + +#include +#include +#include +#include +#include + +#include "ros/ros.h" +#include +#include + +using std::cout; +using std::endl; +using namespace KDL; + +Arm arm("/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/model.sdf"); +KDL::Chain chain = arm.getArm(); +KDL::Chain correct_chain; + +void angle_callback(const simulation_env::arm_angles &msg) +{ + // Create solver based on kinematic chain + static ChainFkSolverPos_recursive fksolver = KDL::ChainFkSolverPos_recursive(chain); + + // Create joint array + unsigned int nj = chain.getNrOfJoints(); + KDL::JntArray jointpositions = KDL::JntArray(nj); + + jointpositions(0) = msg.shoulder_pivot_angle; + jointpositions(1) = msg.shoulder_joint_angle; + jointpositions(2) = msg.elbow_joint_angle; + jointpositions(3) = msg.wrist_pivot_angle; + jointpositions(4) = msg.wrist_joint_angle; + jointpositions(5) = msg.end_effector_pivot_angle; + + // Create the frame that will contain the results + KDL::Frame cartpos; + + // Calculate forward position kinematics + bool kinematics_status; + kinematics_status = fksolver.JntToCart(jointpositions, cartpos); + if (kinematics_status >= 0) + { + std::cout << cartpos << std::endl; + //printf("%s \n", "Succes, thanks KDL!"); + } + else + { + //printf("%s \n", "Error: could not calculate forward kinematics :("); + } +} + +int main(int argc, char **argv) +{ + cout << "Starting arm controller..." << endl; + ros::init(argc, argv, "arm_control_test"); + + // correct.addSegment(Segment(Joint(Joint::RotZ), Frame(Vector(0.0, 0.0, 0.0)))); + // correct.addSegment(Segment(Joint(Joint::RotX), Frame(Vector(0, 0, 1)))); + // correct.addSegment(Segment(Joint(Joint::RotX), Frame(Vector(0, 0, 1)))); + + ros::NodeHandle nh; + // publisher = nh.advertise("/arm_node/arm_commands", 1000); + ros::Subscriber sub = nh.subscribe("/arm_node/arm_positions", 1000, angle_callback); + + // Assign some values to the joint positions + // for (unsigned int i = 0; i < nj; i++) + // { + // float myinput; + // // printf("Enter the position of joint %i: ", i); + // // scanf("%e", &myinput); + // jointpositions(i) = (double)0.0; + // } + + ros::spin(); + cout << "Arm controller finished." << endl; +} \ No newline at end of file diff --git a/rrrobot_src/src/arm_control/test/test.cpp b/rrrobot_src/src/arm_control/test/test.cpp new file mode 100644 index 0000000..886a9f9 --- /dev/null +++ b/rrrobot_src/src/arm_control/test/test.cpp @@ -0,0 +1,61 @@ +// Copyright (C) 2007 Francois Cauwe + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +#include +#include +#include +#include +#include +#include + +using namespace KDL; + +int main(int argc, char **argv) +{ + //Definition of a kinematic chain & add segments to the chain + KDL::Chain chain; + chain.addSegment(Segment(Joint(Joint::RotZ), Frame(Vector(0.0, 0.0, 0.0)))); + chain.addSegment(Segment(Joint(Joint::RotX), Frame(Vector(0, 0, 1)))); + chain.addSegment(Segment(Joint(Joint::RotX), Frame(Vector(0, 0, 1)))); + // chain.addSegment(Segment(Joint(Joint::RotX), Frame(Vector(0.0, 0.0, 0.480)))); + // chain.addSegment(Segment(Joint(Joint::RotX), Frame(Vector(0.0, 0.0, 0.645)))); + // chain.addSegment(Segment(Joint(Joint::RotZ))); + // chain.addSegment(Segment(Joint(Joint::RotX), Frame(Vector(0.0, 0.0, 0.120)))); + // chain.addSegment(Segment(Joint(Joint::RotZ))); + + // Create solver based on kinematic chain + ChainFkSolverPos_recursive fksolver = ChainFkSolverPos_recursive(chain); + + // Create joint array + unsigned int nj = chain.getNrOfJoints(); + KDL::JntArray jointpositions = JntArray(nj); + + // Assign some values to the joint positions + for (unsigned int i = 0; i < nj; i++) + { + float myinput; + printf("Enter the position of joint %i: ", i); + scanf("%e", &myinput); + jointpositions(i) = (double)myinput; + } + + // Create the frame that will contain the results + KDL::Frame cartpos; + + // Calculate forward position kinematics + bool kinematics_status; + kinematics_status = fksolver.JntToCart(jointpositions, cartpos); + if (kinematics_status >= 0) + { + std::cout << cartpos << std::endl; + printf("%s \n", "Succes, thanks KDL!"); + } + else + { + printf("%s \n", "Error: could not calculate forward kinematics :("); + } +} \ No newline at end of file diff --git a/rrrobot_src/src/simulation_env/CMakeLists.txt b/rrrobot_src/src/simulation_env/CMakeLists.txt index 5be384d..78f75d7 100644 --- a/rrrobot_src/src/simulation_env/CMakeLists.txt +++ b/rrrobot_src/src/simulation_env/CMakeLists.txt @@ -137,7 +137,8 @@ add_library(arm_motors SHARED ## Add cmake target dependencies of the library ## as an example, code may need to be generated before libraries ## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(arm_angles ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(arm_motors ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ## Declare a C++ executable ## With catkin_make all packages are built within a single CMake context @@ -191,7 +192,7 @@ target_link_libraries(test_move_arm ## Mark libraries for installation ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -install(TARGETS arm_angles +install(TARGETS arm_angles arm_motors # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION /home/rrrobot/rrrobot_src/lib/ # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}