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

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

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

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

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"

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");

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

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