sravan/RRRobot
1
0
Fork 0
mirror of https://github.com/EECS-467-W20-RRRobot-Project/RRRobot.git synced 2025-08-27 18:12:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

Implemented initial dynamics model which is read in from an sdf file. There are currently some issues with coordinate transforms.

Browse Source
This commit is contained in:
Derek Witcpalek
2020-04-07 22:45:22 -04:00
parent 6f5b2f6142
commit 8e25bb09a3
7 changed files with 827 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

233
rrrobot_src/src/arm_control/CMakeLists.txt Normal file
View File

@@ -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)

73
rrrobot_src/src/arm_control/include/arm_control/arm.h Normal file
View File

@@ -0,0 +1,73 @@
#include <kdl/chain.hpp>
#include <kdl/segment.hpp>
#include <kdl/rigidbodyinertia.hpp>
#include <kdl/rotationalinertia.hpp>
#include <kdl/joint.hpp>
#include <kdl/frames.hpp>
#include <LinearMath/btTransform.h>
#include <string>
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<float> getRequiredTorques(/*std::vector<float> theta_des, std::vector<float> theta_dot_des, */ std::vector<float> 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);
};

65
rrrobot_src/src/arm_control/package.xml Normal file
View File

@@ -0,0 +1,65 @@
<?xml version="1.0"?>
<package format="2">
<name>arm_control</name>
<version>0.0.0</version>
<description>The arm_control package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="dwitcpa@todo.todo">dwitcpa</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/arm_control</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>std_msgs</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>std_msgs</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

313
rrrobot_src/src/arm_control/src/arm.cpp Normal file
View File

@@ -0,0 +1,313 @@
#include <arm.h>
#include <sdf/sdf.hh>
#include <ignition/math/Pose3.hh>
#include <LinearMath/btTransform.h>
#include <iostream>
#include <sstream>
#include <unordered_map>
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<string>("name");
cout << "Found " << model_name << " model" << endl;
unordered_map<string, KDL::Segment> links;
unordered_map<string, string> link_ordering;
string first_link;
// start reading links and joints
sdf::ElementPtr link = model->GetElement("link");
while (link)
{
cout << "Link: " << link->Get<string>("name") << endl;
const string name(link->Get<string>("name"));
KDL::Joint cur_joint(KDL::Joint::None);
const sdf::ElementPtr inertial_data = link->GetElement("inertial");
float mass = inertial_data->Get<float>("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<float>("ixx"),
inertia->Get<float>("iyy"),
inertia->Get<float>("izz"),
inertia->Get<float>("ixy"),
inertia->Get<float>("ixz"),
inertia->Get<float>("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<float>("ixx") << endl;
// cout << inertia->Get<float>("iyy") << endl;
// cout << inertia->Get<float>("izz") << endl;
// cout << inertia->Get<float>("ixy") << endl;
// cout << inertia->Get<float>("ixz") << endl;
// cout << inertia->Get<float>("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<ignition::math::Pose3>() << endl;
// cout << pose << endl;
// cout << *pose_elt->GetValue() << endl;
// sdf::ElementPtr cur = pose_elt->GetFirstElement();
// while (cur)
// {
// cout << cur->Get<float>() << endl;
// cur = cur->GetNextElement();
// }
//ignition::math::Pose3 inertial_pose = inertial_data->Get<ignition::math::Pose3>("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<float>("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<string>("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<float> Arm::getRequiredTorques(/*std::vector<float> theta_des, std::vector<float> theta_dot_des, */ std::vector<float> 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)
{
}

79
rrrobot_src/src/arm_control/src/arm_controller.cpp Normal file
View File

@@ -0,0 +1,79 @@
#include <arm.h>
#include <iostream>
#include <kdl/chain.hpp>
#include <kdl/chainfksolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/frames_io.hpp>
#include <stdio.h>
#include "ros/ros.h"
#include <simulation_env/arm_command.h>
#include <simulation_env/arm_angles.h>
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<simulation_env::arm_command>("/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;
}

61
rrrobot_src/src/arm_control/test/test.cpp Normal file
View File

@@ -0,0 +1,61 @@
// Copyright (C) 2007 Francois Cauwe <francois at cauwe dot org>
// 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 <kdl/chain.hpp>
#include <kdl/chainfksolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/frames_io.hpp>
#include <stdio.h>
#include <iostream>
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 :(");
}
}

5
rrrobot_src/src/simulation_env/CMakeLists.txt
View File

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