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

Changes to arm controller to get it closer to working, and some tests for verifying the correctness of the KDL arm representation

Browse Source
This commit is contained in:
Derek Witcpalek
2020-04-25 17:28:06 -04:00
parent d7918b49a0
commit 8bc0046e7b
7 changed files with 732 additions and 189 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
src/rrrobot_ws/src/rrrobot/CMakeLists.txt
View File

@@ -73,10 +73,14 @@ add_executable(rrrobot_node src/rrrobot_node.cpp)
add_dependencies(rrrobot_node ${catkin_EXPORTED_TARGETS} rrrobot_generate_messages_cpp)
target_link_libraries(rrrobot_node ${catkin_LIBRARIES})
add_executable(arm_controller_node src/arm_controller_node.cpp)
add_executable(arm_controller_node src/arm_controller_node.cpp src/arm_representation.cpp)
add_dependencies(arm_controller_node ${catkin_EXPORTED_TARGETS})
target_link_libraries(arm_controller_node ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES})
add_executable(test_arm test/test_arm.cpp src/arm_representation.cpp)
add_dependencies(test_arm ${catkin_EXPORTED_TARGETS})
target_link_libraries(test_arm ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES})
add_executable(depth_camera_node src/depth_camera_node.cpp)
add_dependencies(depth_camera_node ${catkin_EXPORTED_TARGETS})
target_link_libraries(depth_camera_node ${catkin_LIBRARIES} ${PCL_LIBRARIES})

59
src/rrrobot_ws/src/rrrobot/include/arm_representation.h Normal file
View File

@@ -0,0 +1,59 @@
// arm_controller_node.cpp
#include <algorithm>
#include <vector>
#include <string>
#include <memory>
#include <ros/ros.h>
#include <std_msgs/Float32.h>
#include <std_msgs/String.h>
#include <std_srvs/Trigger.h>
#include <osrf_gear/VacuumGripperControl.h>
#include <trajectory_msgs/JointTrajectory.h>
#include "topic_names.h"
#include "rrrobot/arm_command.h"
#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 <kdl/chainfksolver.hpp>
#include <kdl/chainiksolverpos_lma.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/chainidsolver_recursive_newton_euler.hpp>
#include <fstream>
#include <iostream>
#include <string>
// using namespace std;
using std::string;
class ArmRepresentation
{
public:
ArmRepresentation(const KDL::Frame &base_pose = KDL::Frame(KDL::Vector(0.3, 0.0, 0.9))); //KDL::Frame(KDL::Vector(0.3, 0.92, 1)));
int calculateForwardKinematics(const KDL::JntArray &joint_positions, KDL::Frame &end_effector_pose);
int calculateInverseKinematics(const KDL::JntArray &cur_configuration,
const KDL::Frame &desired_end_effector_pose,
KDL::JntArray &final_joint_configuration);
std::vector<string> get_joint_names();
KDL::Chain *getChain();
private:
KDL::Chain chain;
// KDL::ChainFkSolverPos_recursive fk_solver;
// KDL::ChainIkSolverPos_LMA ik_solver;
};

23
src/rrrobot_ws/src/rrrobot/scripts/arm_pub_test.sh Normal file → Executable file
View File

@@ -1,3 +1,24 @@
#!/bin/bash
rostopic pub /target_pose geometry_msgs/Pose '{position: {x: 0.875, y: 0.75, z: 1.5}, orientation: {x: 0, y: 0, z: 0, w: 0}}'
#rostopic pub /target_pose geometry_msgs/Pose '{position: {x: 0.875, y: 0.75, z: 1.5}, orientation: {x: 0, y: 0, z: 0, w: 0}}'
rostopic pub /arm_controller/destination rrrobot/arm_command "grab_location:
position:
x: 1.2
y: 0.2
z: 1.0
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 0.0
drop_location:
position:
x: -0.3
y: 1.15
z: 1.5
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 0.0" \
-1

23
src/rrrobot_ws/src/rrrobot/scripts/arm_pub_test_continuous.sh Executable file
View File

@@ -0,0 +1,23 @@
#!/bin/bash
#rostopic pub /target_pose geometry_msgs/Pose '{position: {x: 0.875, y: 0.75, z: 1.5}, orientation: {x: 0, y: 0, z: 0, w: 0}}'
rostopic pub /arm_controller/destination rrrobot/arm_command "grab_location:
position:
x: 1.2
y: 0.2
z: 1.0
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 0.0
drop_location:
position:
x: -0.3
y: 1.15
z: 1.5
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 0.0"

452
src/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp
View File

@@ -18,6 +18,7 @@
#include "topic_names.h"
#include "rrrobot/arm_command.h"
#include "arm_representation.h"
#include <kdl/chain.hpp>
#include <kdl/segment.hpp>
@@ -31,191 +32,227 @@
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/chainidsolver_recursive_newton_euler.hpp>
#include <ros/console.h>
#include <cstdlib>
using namespace std;
class ArmRepresentation
// class ArmRepresentation
// {
// public:
// ArmRepresentation(const KDL::Frame &base_pose = KDL::Frame(KDL::Vector(0.3, 0.92, 1)))
// {
// const double base_len = 0.2;
// const double shoulder_height = 0.1273;
// const double upper_arm_length = 0.612;
// const double forearm_length = 0.5723;
// const double shoulder_offset = 0.220941;
// const double elbow_offset = -0.1719;
// const double wrist_1_length = 0.163941 - elbow_offset - shoulder_offset;
// const double wrist_2_length = 0.1157;
// const double wrist_3_length = 0.0922;
// // KDL::Vector pos; //(0, 0, base_len/2);
// // KDL::Rotation rot;
// // The joints might be off by one
// KDL::Segment linear_arm_actuator("linear_arm_actuator_joint",
// KDL::Joint(KDL::Joint::JointType::None), base_pose);
// arm.addSegment(linear_arm_actuator);
// const KDL::Vector pos_base(0, 0, base_len / 2);
// const KDL::Rotation rot_base(KDL::Rotation::RPY(0, 0, 0));
// KDL::Segment base_link("base_link", KDL::Joint("linear_arm_actuator_joint", KDL::Joint::JointType::TransY),
// KDL::Frame(rot_base, pos_base));
// arm.addSegment(base_link);
// const KDL::Vector pos_shoulder(0, 0, shoulder_height);
// const KDL::Rotation rot_shoulder(KDL::Rotation::RPY(0, 0, 0));
// KDL::Segment shoulder_link("shoulder_link", KDL::Joint("shoulder_pan_joint", KDL::Joint::JointType::RotZ),
// KDL::Frame(rot_shoulder, pos_shoulder));
// arm.addSegment(shoulder_link);
// const KDL::Vector pos_upper_arm(0, shoulder_offset, 0);
// const KDL::Rotation rot_upper_arm(KDL::Rotation::RPY(0.0, M_PI / 2.0, 0.0));
// KDL::Segment upper_arm_link("upper_arm_link", KDL::Joint("shoulder_lift_joint", KDL::Joint::JointType::RotY),
// KDL::Frame(rot_upper_arm, pos_upper_arm));
// arm.addSegment(upper_arm_link);
// const KDL::Vector pos_forearm(0, elbow_offset, upper_arm_length);
// const KDL::Rotation rot_forearm(KDL::Rotation::RPY(0.0, 0.0, 0.0));
// KDL::Segment forearm_link("forearm_link", KDL::Joint("elbow_joint", KDL::Joint::JointType::RotY),
// KDL::Frame(rot_forearm, pos_forearm));
// arm.addSegment(forearm_link);
// const KDL::Vector pos_wrist_1(0, 0, forearm_length);
// const KDL::Rotation rot_wrist_1(KDL::Rotation::RPY(0.0, M_PI / 2.0, 0.0));
// KDL::Segment wrist_1_link("wrist_1_link", KDL::Joint("wrist_1_joint", KDL::Joint::JointType::RotY),
// KDL::Frame(rot_wrist_1, pos_wrist_1));
// arm.addSegment(wrist_1_link);
// const KDL::Vector pos_wrist_2(0, wrist_1_length, 0);
// const KDL::Rotation rot_wrist_2(KDL::Rotation::RPY(0.0, 0.0, 0.0));
// KDL::Segment wrist_2_link("wrist_2_link", KDL::Joint("wrist_2_joint", KDL::Joint::JointType::RotZ),
// KDL::Frame(rot_wrist_2, pos_wrist_2));
// arm.addSegment(wrist_2_link);
// const KDL::Vector pos_wrist_3(0, 0, wrist_2_length);
// const KDL::Rotation rot_wrist_3(KDL::Rotation::RPY(0.0, 0.0, 0.0));
// KDL::Segment wrist_3_link("wrist_3_link", KDL::Joint("wrist_3_joint", KDL::Joint::JointType::RotY),
// KDL::Frame(rot_wrist_3, pos_wrist_3));
// arm.addSegment(wrist_3_link);
// const KDL::Vector pos_ee(0, wrist_3_length, 0.0);
// const KDL::Rotation rot_ee(KDL::Rotation::RPY(0.0, 0.0, M_PI / 2.0));
// KDL::Segment ee_link("ee_link", KDL::Joint(KDL::Joint::JointType::None),
// KDL::Frame(rot_ee, pos_ee));
// arm.addSegment(ee_link);
// // arm.addSegment(base_link);
// // arm.addSegment(shoulder_link);
// // arm.addSegment(upper_arm_link);
// // arm.addSegment(forearm_link);
// // arm.addSegment(wrist_1_link);
// // arm.addSegment(wrist_2_link);
// // arm.addSegment(wrist_3_link);
// // arm.addSegment(ee_link);
// ROS_INFO("Arm has %d joints", arm->getNrOfJoints());
// fk_solver.reset(new KDL::ChainFkSolverPos_recursive(arm));
// ik_solver.reset(new KDL::ChainIkSolverPos_LMA(arm));
// }
// int calculateForwardKinematics(const KDL::JntArray &joint_positions, KDL::Frame &end_effector_pose)
// {
// return fk_solver->JntToCart(joint_positions, end_effector_pose);
// }
// int calculateInverseKinematics(const KDL::JntArray &cur_configuration,
// const KDL::Frame &desired_end_effector_pose,
// KDL::JntArray &final_joint_configuration)
// {
// return ik_solver->CartToJnt(cur_configuration, desired_end_effector_pose, final_joint_configuration);
// }
// vector<string> get_joint_names()
// {
// vector<string> joints;
// for (int segment = 0; segment < arm->getNrOfSegments(); ++segment)
// {
// KDL::Joint cur = arm.getSegment(segment).getJoint();
// if (cur.getType() != KDL::Joint::JointType::None)
// {
// joints.push_back(cur.getName());
// }
// }
// return joints;
// }
// const KDL::Chain &getChain() const
// {
// return arm;
// }
// private:
// KDL::Chain arm;
// std::unique_ptr<KDL::ChainFkSolverPos_recursive> fk_solver;
// std::unique_ptr<KDL::ChainIkSolverPos_LMA> ik_solver;
// };
class ArmController
{
public:
ArmRepresentation(const KDL::Frame &base_pose = KDL::Frame(KDL::Vector(0.3, 0.92, 1)))
ArmController(ros::NodeHandle &node, ArmRepresentation *arm_) : gripper_enabled_(false), item_attached_(false), arm(arm_)
{
const double base_len = 0.2;
const double shoulder_height = 0.1273;
const double upper_arm_length = 0.612;
const double forearm_length = 0.5723;
const double shoulder_offset = 0.220941;
const double elbow_offset = -0.1719;
const double wrist_1_length = 0.163941 - elbow_offset - shoulder_offset;
const double wrist_2_length = 0.1157;
const double wrist_3_length = 0.0922;
// KDL::Vector pos; //(0, 0, base_len/2);
// KDL::Rotation rot;
// The joints might be off by one
KDL::Segment linear_arm_actuator("linear_arm_actuator_joint",
KDL::Joint(KDL::Joint::JointType::None), base_pose);
arm.addSegment(linear_arm_actuator);
const KDL::Vector pos_base(0, 0, base_len / 2);
const KDL::Rotation rot_base(KDL::Rotation::RPY(0, 0, 0));
KDL::Segment base_link("base_link", KDL::Joint(KDL::Joint::JointType::TransY),
KDL::Frame(rot_base, pos_base));
arm.addSegment(base_link);
const KDL::Vector pos_shoulder(0, 0, shoulder_height);
const KDL::Rotation rot_shoulder(KDL::Rotation::RPY(0, 0, 0));
KDL::Segment shoulder_link("shoulder_link", KDL::Joint(KDL::Joint::JointType::RotZ),
KDL::Frame(rot_shoulder, pos_shoulder));
arm.addSegment(shoulder_link);
const KDL::Vector pos_upper_arm(0, shoulder_offset, 0);
const KDL::Rotation rot_upper_arm(KDL::Rotation::RPY(0.0, M_PI / 2.0, 0.0));
KDL::Segment upper_arm_link("upper_arm_link", KDL::Joint(KDL::Joint::JointType::RotY),
KDL::Frame(rot_upper_arm, pos_upper_arm));
arm.addSegment(upper_arm_link);
const KDL::Vector pos_forearm(0, elbow_offset, upper_arm_length);
const KDL::Rotation rot_forearm(KDL::Rotation::RPY(0.0, 0.0, 0.0));
KDL::Segment forearm_link("forearm_link", KDL::Joint(KDL::Joint::JointType::RotY),
KDL::Frame(rot_forearm, pos_forearm));
arm.addSegment(forearm_link);
const KDL::Vector pos_wrist_1(0, 0, forearm_length);
const KDL::Rotation rot_wrist_1(KDL::Rotation::RPY(0.0, M_PI / 2.0, 0.0));
KDL::Segment wrist_1_link("wrist_1_link", KDL::Joint(KDL::Joint::JointType::RotY),
KDL::Frame(rot_wrist_1, pos_wrist_1));
arm.addSegment(wrist_1_link);
const KDL::Vector pos_wrist_2(0, wrist_1_length, 0);
const KDL::Rotation rot_wrist_2(KDL::Rotation::RPY(0.0, 0.0, 0.0));
KDL::Segment wrist_2_link("wrist_2_link", KDL::Joint(KDL::Joint::JointType::RotZ),
KDL::Frame(rot_wrist_2, pos_wrist_2));
arm.addSegment(wrist_2_link);
const KDL::Vector pos_wrist_3(0, 0, wrist_2_length);
const KDL::Rotation rot_wrist_3(KDL::Rotation::RPY(0.0, 0.0, 0.0));
KDL::Segment wrist_3_link("wrist_3_link", KDL::Joint(KDL::Joint::JointType::RotY),
KDL::Frame(rot_wrist_3, pos_wrist_3));
arm.addSegment(wrist_3_link);
const KDL::Vector pos_ee(0, wrist_3_length, 0.0);
const KDL::Rotation rot_ee(KDL::Rotation::RPY(0.0, 0.0, M_PI / 2.0));
KDL::Segment ee_link("ee_link", KDL::Joint(KDL::Joint::JointType::None),
KDL::Frame(rot_ee, pos_ee));
arm.addSegment(ee_link);
// arm.addSegment(base_link);
// arm.addSegment(shoulder_link);
// arm.addSegment(upper_arm_link);
// arm.addSegment(forearm_link);
// arm.addSegment(wrist_1_link);
// arm.addSegment(wrist_2_link);
// arm.addSegment(wrist_3_link);
// arm.addSegment(ee_link);
fk_solver.reset(new KDL::ChainFkSolverPos_recursive(arm));
ik_solver.reset(new KDL::ChainIkSolverPos_LMA(arm));
}
bool calculateForwardKinematics(const KDL::JntArray &joint_positions, KDL::Frame &end_effector_pose)
{
return fk_solver->JntToCart(joint_positions, end_effector_pose);
}
bool calculateInverseKinematics(const KDL::JntArray &cur_configuration,
const KDL::Frame &desired_end_effector_pose,
KDL::JntArray &final_joint_configuration)
{
return ik_solver->CartToJnt(cur_configuration, desired_end_effector_pose, final_joint_configuration);
}
vector<string> get_joint_names() {
vector<string> joints;
for (int segment = 0; segment < arm.getNrOfSegments(); ++segment) {
KDL::Joint cur = arm.getSegment(segment).getJoint();
if (cur.getType() != KDL::Joint::JointType::None)
{
joints.push_back(cur.getName());
}
}
return joints;
}
const KDL::Chain &getArm() const
{
return arm;
}
private:
KDL::Chain arm;
std::unique_ptr<KDL::ChainFkSolverPos_recursive> fk_solver;
std::unique_ptr<KDL::ChainIkSolverPos_LMA> ik_solver;
};
class ArmController {
public:
ArmController(ros::NodeHandle & node) : gripper_enabled_(false), item_attached_(false) {
arm_joint_trajectory_publisher_ = node.advertise<trajectory_msgs::JointTrajectory>(ARM_COMMAND_CHANNEL, 10);
gripper_ = node.serviceClient<osrf_gear::VacuumGripperControl>(GRIPPER_CONTROL_CHANNEL);
arm = ArmRepresentation();
// arm = std::move(arm_);
ROS_INFO("Arm size (in constructor): %d", arm->getChain()->getNrOfJoints());
arm_current_joint_states_ = KDL::JntArray(arm->getChain()->getNrOfJoints());
KDL::SetToZero(arm_current_joint_states_);
}
void joint_state_callback(const sensor_msgs::JointState & joint_state_msg) {
ROS_INFO_STREAM_THROTTLE(10, "Arm Joint States (throttled to 0.1 Hz):\n" << joint_state_msg);
// ROS_INFO_STREAM("Joint States:\n" << joint_state_msg);
void joint_state_callback(const sensor_msgs::JointState &joint_state_msg)
{
// ROS_INFO("Received joint state");
// Convert std_msgs::double[] to KDL::JntArray
int nbr_joints = arm.getArm().getNrOfJoints();
int nbr_joints = arm->getChain()->getNrOfJoints();
vector<string> msg_joint_names = joint_state_msg.name;
vector<string> cur_joint_names = arm.get_joint_names();
vector<string> cur_joint_names = arm->get_joint_names();
vector<double> position = joint_state_msg.position;
for (int i = 0; i < nbr_joints; ++i) {
for (int j = 0; i < nbr_joints; ++j) {
if (msg_joint_names[i].compare(cur_joint_names[j]) == 0) {
// Print joint name vectors for debugging
// ROS_INFO_STREAM("msg_joint_names: {");
// for (int i = 0; i < nbr_joints; ++i)
// {
// ROS_INFO_STREAM(msg_joint_names[i] << ", ");
// }
// ROS_INFO_STREAM("}\ncur_joint_names: {");
// for (int j = 0; j < cur_joint_names.size(); ++j)
// {
// ROS_INFO_STREAM(cur_joint_names[j] << ", ");
// }
// ROS_INFO_STREAM("}");
for (int i = 0; i < nbr_joints; ++i)
{
arm_current_joint_states_(i) = 0.0;
}
for (size_t i = 0; i < position.size(); ++i)
{
for (size_t j = 0; j < cur_joint_names.size(); ++j)
{
if (msg_joint_names[i].compare(cur_joint_names[j]) == 0)
{
arm_current_joint_states_(j) = position[i];
}
}
}
}
void gripper_state_callback(const osrf_gear::VacuumGripperState::ConstPtr & gripper_state_msg) {
void gripper_state_callback(const osrf_gear::VacuumGripperState::ConstPtr &gripper_state_msg)
{
gripper_enabled_ = gripper_state_msg->enabled;
item_attached_ = gripper_state_msg->attached;
}
void arm_destination_callback(const rrrobot::arm_command & target_pose) {
int nbr_joints = arm.getArm().getNrOfJoints();
void arm_destination_callback(const rrrobot::arm_command &target_pose)
{
ROS_INFO("Received target pose");
int nbr_joints = arm->getChain()->getNrOfJoints();
vector<double> positions;
double time_from_start = 5; // Seconds
// Move arm to pickup item from conveyor belt
positions = calc_joint_positions(target_pose.grab_location);
send_joint_trajectory(positions, time_from_start);
// Wait until target position is reached
while (!have_reached_target(frame_to_pose(calc_end_effector_pose()), target_pose.grab_location)) {
while (!have_reached_target(frame_to_pose(calc_end_effector_pose()), target_pose.grab_location))
{
// TODO: Do something if not able to reach target
continue;
}
// Turn on suction
while (!gripper_enabled_) {
while (!gripper_enabled_)
{
gripper_control(true);
}
// Wait until object is attached
while (!item_attached_) {
while (!item_attached_)
{
// TODO: Do something if item is not able to attach
continue;
}
@@ -223,20 +260,23 @@ public:
// Move item to desired position
positions = calc_joint_positions(target_pose.drop_location);
send_joint_trajectory(positions, time_from_start);
// Wait until target position is reached
while (!have_reached_target(frame_to_pose(calc_end_effector_pose()), target_pose.drop_location)) {
while (!have_reached_target(frame_to_pose(calc_end_effector_pose()), target_pose.drop_location))
{
// TODO: Do something if not able to reach target
continue;
}
// Turn off suction
while (gripper_enabled_) {
while (gripper_enabled_)
{
gripper_control(false);
}
// Wait until object is detached
while (item_attached_) {
while (item_attached_)
{
// TODO: Do something if item doesn't detach
continue;
}
@@ -248,39 +288,60 @@ private:
ros::Publisher arm_joint_trajectory_publisher_;
ros::ServiceClient gripper_;
KDL::JntArray arm_current_joint_states_;
ArmRepresentation arm;
ArmRepresentation *arm;
vector<double> calc_joint_positions(const geometry_msgs::Pose & pose) {
KDL::JntArray cur_configuration = arm_current_joint_states_;
vector<double> calc_joint_positions(const geometry_msgs::Pose &pose)
{
// KDL::JntArray cur_configuration(arm->getChain()->getNrOfJoints()); // = arm_current_joint_states_;
// KDL::SetToZero(cur_configuration);
KDL::Frame desired_end_effector_pose = pose_to_frame(pose);
KDL::JntArray final_joint_configuration = KDL::JntArray();
KDL::JntArray final_joint_configuration(arm->getChain()->getNrOfJoints());
// ROS_INFO("cur_configuration (%i x %i)", cur_configuration.rows(), cur_configuration.columns());
int error_code = arm->calculateInverseKinematics(arm_current_joint_states_, desired_end_effector_pose, final_joint_configuration);
// Check status of IK and print error message if there is a failure
if (!arm.calculateInverseKinematics(cur_configuration, desired_end_effector_pose, final_joint_configuration)) {
ROS_ERROR("Inverse Kinematics Failure");
if (error_code != 0)
{
ROS_ERROR("Inverse Kinematics Failure: %i", error_code);
}
// Convert data attribute (Eigen::VectorXd) of KDL::JntArray to double[] via data() function
int nbr_joints = arm.getArm().getNrOfJoints();
int nbr_joints = arm->getChain()->getNrOfJoints();
Eigen::VectorXd mat = final_joint_configuration.data;
vector<double> positions(mat.data(), mat.data() + mat.rows() * mat.cols());
// cout << mat << endl;
vector<double> positions;
for (size_t idx = 0; idx < arm->getChain()->getNrOfJoints(); ++idx)
{
positions.push_back(mat[idx]);
}
return positions;
}
const KDL::Frame& calc_end_effector_pose() {
KDL::Frame calc_end_effector_pose()
{
KDL::Frame end_effector_pose;
KDL::JntArray joint_positions = arm_current_joint_states_;
// KDL::JntArray joint_positions = arm_current_joint_states_;
KDL::JntArray joint_positions(arm->getChain()->getNrOfJoints());
KDL::SetToZero(joint_positions);
// ROS_INFO("joint_positions (%i x %i)", joint_positions.rows(), joint_positions.columns());
// ROS_INFO("arm chain # of joints: %d", arm->getChain()->getNrOfJoints());
int error_code = arm->calculateForwardKinematics(joint_positions, end_effector_pose);
// Check status of FK and do something if there is a failure
if (!arm.calculateForwardKinematics(joint_positions, end_effector_pose)) {
ROS_ERROR("Forward Kinematics Failure");
if (error_code != 0)
{
ROS_ERROR("Forward Kinematics Failure: %i", error_code);
}
return end_effector_pose;
}
const KDL::Frame& pose_to_frame(const geometry_msgs::Pose & pose) {
KDL::Frame pose_to_frame(const geometry_msgs::Pose &pose)
{
double p_x = pose.position.x;
double p_y = pose.position.y;
double p_z = pose.position.z;
@@ -290,10 +351,13 @@ private:
double q_z = pose.orientation.z;
double q_w = pose.orientation.w;
return KDL::Frame(KDL::Rotation::Quaternion(q_x, q_y, q_z, q_w), KDL::Vector(p_x, p_y, p_z));
KDL::Rotation rot(KDL::Rotation::Quaternion(q_x, q_y, q_z, q_w));
KDL::Vector pos(p_x, p_y, p_z);
return KDL::Frame(rot, pos);
}
const geometry_msgs::Pose& frame_to_pose(const KDL::Frame & frame) {
geometry_msgs::Pose frame_to_pose(const KDL::Frame &frame)
{
double p_x = frame.p.x();
double p_y = frame.p.y();
double p_z = frame.p.z();
@@ -315,35 +379,42 @@ private:
return pose;
}
void send_joint_trajectory(vector<double> & positions, double time_from_start) {
void send_joint_trajectory(const vector<double> &positions, double time_from_start)
{
// Declare JointTrajectory message
trajectory_msgs::JointTrajectory msg;
// Fill the names of the joints to be controlled
msg.joint_names = arm.get_joint_names();
msg.joint_names = arm->get_joint_names();
// Create one point in the trajectory
msg.points.resize(1);
msg.points[0].positions.resize(msg.joint_names.size(), 0.0);
// Set joint positions
msg.points[0].positions = positions;
for (size_t idx = 0; idx < positions.size(); ++idx)
{
msg.points[0].positions[idx] = positions[idx];
}
// How long to take getting to the point (floating point seconds)
msg.points[0].time_from_start = ros::Duration(time_from_start);
ROS_INFO_STREAM("Sending command:\n" << msg);
//ROS_INFO("Sending command: \n%s", msg);
arm_joint_trajectory_publisher_.publish(msg);
}
void gripper_control(bool state) {
void gripper_control(bool state)
{
osrf_gear::VacuumGripperControl srv;
srv.request.enable = state;
gripper_.call(srv);
}
bool have_reached_target(geometry_msgs::Pose cur, geometry_msgs::Pose target) {
bool have_reached_target(geometry_msgs::Pose cur, geometry_msgs::Pose target)
{
// TODO: Tune threshold values
float pos_thresh = 0.01; // Meters
float rot_thresh = 0.02; // Radians
@@ -351,8 +422,9 @@ private:
float pos_err = fabs(cur.position.x - target.position.x) +
fabs(cur.position.y - target.position.y) +
fabs(cur.position.z - target.position.z);
if (pos_err > pos_thresh) {
if (pos_err > pos_thresh)
{
return false;
}
@@ -360,20 +432,24 @@ private:
float qy_err = fabs(cur.orientation.y - target.orientation.y);
float qz_err = fabs(cur.orientation.z - target.orientation.z);
float qw_err = fabs(cur.orientation.w - target.orientation.w);
if (qx_err > rot_thresh) {
if (qx_err > rot_thresh)
{
return false;
}
if (qy_err > rot_thresh) {
if (qy_err > rot_thresh)
{
return false;
}
if (qz_err > rot_thresh) {
if (qz_err > rot_thresh)
{
return false;
}
if (qw_err > rot_thresh) {
if (qw_err > rot_thresh)
{
return false;
}
@@ -381,22 +457,24 @@ private:
}
};
int main(int argc, char ** argv) {
int main(int argc, char **argv)
{
cout << "Starting arm_controller_node" << endl;
// Last argument is the default name of the node.
ros::init(argc, argv, "arm_controller_node");
ros::NodeHandle node;
ArmController ac(node);
ArmRepresentation arm;
ArmController ac(node, &arm);
// Subscribe to arm destination and joint states channels
ros::Subscriber arm_destination_sub = node.subscribe(ARM_DESTINATION_CHANNEL, 1, &ArmController::arm_destination_callback, &ac);
ros::Subscriber gripper_state_sub = node.subscribe(GRIPPER_STATE_CHANNEL, 10, &ArmController::gripper_state_callback, &ac);
ros::Subscriber joint_state_sub = node.subscribe(ARM_JOINT_STATES_CHANNEL, 10, &ArmController::joint_state_callback, &ac);
ROS_INFO("Setup complete");

127
src/rrrobot_ws/src/rrrobot/src/arm_representation.cpp Normal file
View File

@@ -0,0 +1,127 @@
// arm_controller_node.cpp
#include "arm_representation.h"
// using namespace std;
using std::string;
using std::vector;
ArmRepresentation::ArmRepresentation(const KDL::Frame &base_pose)
{
const double base_len = 0.2;
const double shoulder_height = 0.1273;
const double upper_arm_length = 0.612;
const double forearm_length = 0.5723;
const double shoulder_offset = 0.220941;
const double elbow_offset = -0.1719;
const double wrist_1_length = 0.163941 - elbow_offset - shoulder_offset;
const double wrist_2_length = 0.1157;
const double wrist_3_length = 0.0922;
// KDL::Vector pos; //(0, 0, base_len/2);
// KDL::Rotation rot;
// The joints might be off by one
KDL::Segment linear_arm_actuator("linear_arm_actuator_joint",
KDL::Joint(KDL::Joint::JointType::None), base_pose);
chain.addSegment(linear_arm_actuator);
const KDL::Vector pos_base(0, 0, base_len / 2);
const KDL::Rotation rot_base(KDL::Rotation::RPY(0, 0, 0));
KDL::Segment base_link("base_link", KDL::Joint("linear_arm_actuator_joint", KDL::Joint::JointType::TransY),
KDL::Frame(rot_base, pos_base));
chain.addSegment(base_link);
const KDL::Vector pos_shoulder(0, 0, shoulder_height);
const KDL::Rotation rot_shoulder(KDL::Rotation::RPY(0, 0, 0));
KDL::Segment shoulder_link("shoulder_link", KDL::Joint("shoulder_pan_joint", KDL::Joint::JointType::RotZ),
KDL::Frame(rot_shoulder, pos_shoulder));
chain.addSegment(shoulder_link);
const KDL::Vector pos_upper_arm(0, shoulder_offset, 0);
const KDL::Rotation rot_upper_arm(KDL::Rotation::RPY(0.0, M_PI / 2.0, 0.0));
KDL::Segment upper_arm_link("upper_arm_link", KDL::Joint("shoulder_lift_joint", KDL::Joint::JointType::RotY),
KDL::Frame(rot_upper_arm, pos_upper_arm));
chain.addSegment(upper_arm_link);
const KDL::Vector pos_forearm(0, elbow_offset, upper_arm_length);
const KDL::Rotation rot_forearm(KDL::Rotation::RPY(0.0, 0.0, 0.0));
KDL::Segment forearm_link("forearm_link", KDL::Joint("elbow_joint", KDL::Joint::JointType::RotY),
KDL::Frame(rot_forearm, pos_forearm));
chain.addSegment(forearm_link);
const KDL::Vector pos_wrist_1(0, 0, forearm_length);
const KDL::Rotation rot_wrist_1(KDL::Rotation::RPY(0.0, M_PI / 2.0, 0.0));
KDL::Segment wrist_1_link("wrist_1_link", KDL::Joint("wrist_1_joint", KDL::Joint::JointType::RotY),
KDL::Frame(rot_wrist_1, pos_wrist_1));
chain.addSegment(wrist_1_link);
const KDL::Vector pos_wrist_2(0, wrist_1_length, 0);
const KDL::Rotation rot_wrist_2(KDL::Rotation::RPY(0.0, 0.0, 0.0));
KDL::Segment wrist_2_link("wrist_2_link", KDL::Joint("wrist_2_joint", KDL::Joint::JointType::RotZ),
KDL::Frame(rot_wrist_2, pos_wrist_2));
chain.addSegment(wrist_2_link);
const KDL::Vector pos_wrist_3(0, 0, wrist_2_length);
const KDL::Rotation rot_wrist_3(KDL::Rotation::RPY(0.0, 0.0, 0.0));
KDL::Segment wrist_3_link("wrist_3_link", KDL::Joint("wrist_3_joint", KDL::Joint::JointType::RotY),
KDL::Frame(rot_wrist_3, pos_wrist_3));
chain.addSegment(wrist_3_link);
const KDL::Vector pos_ee(0, wrist_3_length, 0.0);
const KDL::Rotation rot_ee(KDL::Rotation::RPY(0.0, 0.0, M_PI / 2.0));
KDL::Segment ee_link("ee_link", KDL::Joint(KDL::Joint::JointType::None),
KDL::Frame(rot_ee, pos_ee));
chain.addSegment(ee_link);
// arm.addSegment(base_link);
// arm.addSegment(shoulder_link);
// arm.addSegment(upper_arm_link);
// arm.addSegment(forearm_link);
// arm.addSegment(wrist_1_link);
// arm.addSegment(wrist_2_link);
// arm.addSegment(wrist_3_link);
// arm.addSegment(ee_link);
// fk_solver = KDL::ChainFkSolverPos_recursive(chain);
// ik_solver = KDL::ChainIkSolverPos_LMA(chain);
// fk_solver.reset(new KDL::ChainFkSolverPos_recursive(arm));
// ik_solver.reset(new KDL::ChainIkSolverPos_LMA(arm));
}
int ArmRepresentation::calculateForwardKinematics(const KDL::JntArray &joint_positions, KDL::Frame &end_effector_pose)
{
KDL::ChainFkSolverPos_recursive fk_solver = KDL::ChainFkSolverPos_recursive(chain);
int status = fk_solver.JntToCart(joint_positions, end_effector_pose);
return status;
}
int ArmRepresentation::calculateInverseKinematics(const KDL::JntArray &cur_configuration,
const KDL::Frame &desired_end_effector_pose,
KDL::JntArray &final_joint_configuration)
{
KDL::ChainIkSolverPos_LMA ik_solver = KDL::ChainIkSolverPos_LMA(chain);
int status = ik_solver.CartToJnt(cur_configuration, desired_end_effector_pose, final_joint_configuration);
return status;
}
vector<string> ArmRepresentation::get_joint_names()
{
vector<string> joint_names;
for (auto it : chain.segments)
{
if (it.getJoint().getType() != KDL::Joint::JointType::None)
joint_names.push_back(it.getJoint().getName());
}
return joint_names;
}
KDL::Chain *ArmRepresentation::getChain()
{
return &chain;
}

231
src/rrrobot_ws/src/rrrobot/test/test_arm.cpp Normal file
View File

@@ -0,0 +1,231 @@
// arm_controller_node.cpp
#include <algorithm>
#include <vector>
#include <string>
#include <memory>
#include <ros/ros.h>
#include <std_msgs/Float32.h>
#include <std_msgs/String.h>
#include <std_srvs/Trigger.h>
#include <osrf_gear/VacuumGripperControl.h>
#include <trajectory_msgs/JointTrajectory.h>
#include "topic_names.h"
#include "rrrobot/arm_command.h"
#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 <kdl/chainfksolver.hpp>
#include <kdl/chainiksolverpos_lma.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/chainidsolver_recursive_newton_euler.hpp>
#include <kdl/frames_io.hpp>
#include <fstream>
#include <iostream>
#include <string>
#include "arm_representation.h"
// using namespace std;
using std::cin;
using std::cout;
using std::endl;
using std::string;
// class ArmRepresentation
// {
// public:
// ArmRepresentation(const KDL::Frame &base_pose = KDL::Frame(KDL::Vector(0.3, 0.92, 1)))
// {
// const double base_len = 0.2;
// const double shoulder_height = 0.1273;
// const double upper_arm_length = 0.612;
// const double forearm_length = 0.5723;
// const double shoulder_offset = 0.220941;
// const double elbow_offset = -0.1719;
// const double wrist_1_length = 0.163941 - elbow_offset - shoulder_offset;
// const double wrist_2_length = 0.1157;
// const double wrist_3_length = 0.0922;
// // KDL::Vector pos; //(0, 0, base_len/2);
// // KDL::Rotation rot;
// // The joints might be off by one
// KDL::Segment linear_arm_actuator("linear_arm_actuator_joint",
// KDL::Joint(KDL::Joint::JointType::None), base_pose);
// arm.addSegment(linear_arm_actuator);
// const KDL::Vector pos_base(0, 0, base_len / 2);
// const KDL::Rotation rot_base(KDL::Rotation::RPY(0, 0, 0));
// KDL::Segment base_link("base_link", KDL::Joint(KDL::Joint::JointType::TransY),
// KDL::Frame(rot_base, pos_base));
// arm.addSegment(base_link);
// const KDL::Vector pos_shoulder(0, 0, shoulder_height);
// const KDL::Rotation rot_shoulder(KDL::Rotation::RPY(0, 0, 0));
// KDL::Segment shoulder_link("shoulder_link", KDL::Joint(KDL::Joint::JointType::RotZ),
// KDL::Frame(rot_shoulder, pos_shoulder));
// arm.addSegment(shoulder_link);
// const KDL::Vector pos_upper_arm(0, shoulder_offset, 0);
// const KDL::Rotation rot_upper_arm(KDL::Rotation::RPY(0.0, M_PI / 2.0, 0.0));
// KDL::Segment upper_arm_link("upper_arm_link", KDL::Joint(KDL::Joint::JointType::RotY),
// KDL::Frame(rot_upper_arm, pos_upper_arm));
// arm.addSegment(upper_arm_link);
// const KDL::Vector pos_forearm(0, elbow_offset, upper_arm_length);
// const KDL::Rotation rot_forearm(KDL::Rotation::RPY(0.0, 0.0, 0.0));
// KDL::Segment forearm_link("forearm_link", KDL::Joint(KDL::Joint::JointType::RotY),
// KDL::Frame(rot_forearm, pos_forearm));
// arm.addSegment(forearm_link);
// const KDL::Vector pos_wrist_1(0, 0, forearm_length);
// const KDL::Rotation rot_wrist_1(KDL::Rotation::RPY(0.0, M_PI / 2.0, 0.0));
// KDL::Segment wrist_1_link("wrist_1_link", KDL::Joint(KDL::Joint::JointType::RotY),
// KDL::Frame(rot_wrist_1, pos_wrist_1));
// arm.addSegment(wrist_1_link);
// const KDL::Vector pos_wrist_2(0, wrist_1_length, 0);
// const KDL::Rotation rot_wrist_2(KDL::Rotation::RPY(0.0, 0.0, 0.0));
// KDL::Segment wrist_2_link("wrist_2_link", KDL::Joint(KDL::Joint::JointType::RotZ),
// KDL::Frame(rot_wrist_2, pos_wrist_2));
// arm.addSegment(wrist_2_link);
// const KDL::Vector pos_wrist_3(0, 0, wrist_2_length);
// const KDL::Rotation rot_wrist_3(KDL::Rotation::RPY(0.0, 0.0, 0.0));
// KDL::Segment wrist_3_link("wrist_3_link", KDL::Joint(KDL::Joint::JointType::RotY),
// KDL::Frame(rot_wrist_3, pos_wrist_3));
// arm.addSegment(wrist_3_link);
// const KDL::Vector pos_ee(0, wrist_3_length, 0.0);
// const KDL::Rotation rot_ee(KDL::Rotation::RPY(0.0, 0.0, M_PI / 2.0));
// KDL::Segment ee_link("ee_link", KDL::Joint(KDL::Joint::JointType::None),
// KDL::Frame(rot_ee, pos_ee));
// arm.addSegment(ee_link);
// // arm.addSegment(base_link);
// // arm.addSegment(shoulder_link);
// // arm.addSegment(upper_arm_link);
// // arm.addSegment(forearm_link);
// // arm.addSegment(wrist_1_link);
// // arm.addSegment(wrist_2_link);
// // arm.addSegment(wrist_3_link);
// // arm.addSegment(ee_link);
// fk_solver.reset(new KDL::ChainFkSolverPos_recursive(arm));
// ik_solver.reset(new KDL::ChainIkSolverPos_LMA(arm));
// }
// int calculateForwardKinematics(const KDL::JntArray &joint_positions, KDL::Frame &end_effector_pose)
// {
// return fk_solver->JntToCart(joint_positions, end_effector_pose);
// }
// int calculateInverseKinematics(const KDL::JntArray &cur_configuration,
// const KDL::Frame &desired_end_effector_pose,
// KDL::JntArray &final_joint_configuration)
// {
// return ik_solver->CartToJnt(cur_configuration, desired_end_effector_pose, final_joint_configuration);
// }
// string *get_joint_names()
// {
// // TODO: return ["linear_arm_actuator_joint",
// // "shoulder_pan_joint",
// // "shoulder_lift_joint",
// // "elbow_joint",
// // "wrist_1_joint",
// // "wrist_2_joint",
// // "wrist_3_joint"];
// }
// const KDL::Chain &getArm() const
// {
// return arm;
// }
// private:
// KDL::Chain arm;
// std::unique_ptr<KDL::ChainFkSolverPos_recursive> fk_solver;
// std::unique_ptr<KDL::ChainIkSolverPos_LMA> ik_solver;
// };
int main(int argc, char **argv)
{
ArmRepresentation arm;
KDL::JntArray pos(arm.getChain()->getNrOfJoints());
KDL::SetToZero(pos);
int joint;
// std::cout << "Enter joint to exercise: ";
// std::cin >> joint;
KDL::Frame end_effector_pose;
std::ofstream f("data.txt");
int error_code;
for (joint = 0; joint < arm.getChain()->getNrOfJoints(); ++joint)
{
for (double pos_val = 0.0; pos_val <= 2 * M_PI; pos_val += 0.1)
{
pos(joint) = pos_val;
error_code = arm.calculateForwardKinematics(pos, end_effector_pose);
if (error_code != 0)
{
ROS_ERROR("Forward Kinematics Failure: %i", error_code);
}
f << end_effector_pose.p.x() << "," << end_effector_pose.p.y() << "," << end_effector_pose.p.z() << '\n';
error_code = arm.calculateInverseKinematics(pos, end_effector_pose, pos);
if (error_code != 0)
{
ROS_ERROR("Inverse Kinematics Failure: %i", error_code);
}
}
}
double linear_arm_actuator_joint, shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint;
while (true)
{
cout << "linear_arm_actuator_joint: ";
cin >> pos(0); //linear_arm_actuator_joint;
cout << "shoulder_pan_joint: ";
cin >> pos(1); //shoulder_pan_joint;
cout << "shoulder_lift_joint: ";
cin >> pos(2); //shoulder_lift_joint;
cout << "elbow_joint: ";
cin >> pos(3); //elbow_joint;
cout << "wrist_1_joint: ";
cin >> pos(4); //wrist_1_joint;
cout << "wrist_2_joint: ";
cin >> pos(5); //wrist_2_joint;
cout << "wrist_3_joint: ";
cin >> pos(6); //wrist_3_joint;
error_code = arm.calculateForwardKinematics(pos, end_effector_pose);
if (error_code != 0)
{
ROS_ERROR("Forward kinematics failure: %i", error_code);
}
cout << "x,y,z: " << end_effector_pose.p.x() << ", " << end_effector_pose.p.y() << ", " << end_effector_pose.p.z() << endl;
cout << end_effector_pose.M << endl;
}
}