sravan/RRRobot
1
0
Fork 0
mirror of https://github.com/EECS-467-W20-RRRobot-Project/RRRobot.git synced 2025-08-14 05:58:32 +00:00
Code Issues Packages Projects Releases Wiki Activity

Working arm movement to specific pose (x,y,z,roll,pitch,yaw). There are a lot of other changes too.

Browse Source 
-Removed gravity (temporary, I hope). I was struggling to get inverse dynamics working correctly to provide a torque to compensate for gravity. Eventually, I think we want to have a PID, plus gravity compensation.
  -Created a demo world with setpoints that the robot heads to.
  -Arm kinematic representation is working.
  -PID control of each joint is working to a basic extent. Some segments are more responsive than others.

Still to do:
  -reading in SDF should be more robust, and should support all joint types
  -better control
  -obstacle avoidance in arm movement path
  -control with gravity
  -arm joint limits, and shortest path to angle (ie considers that theta wraps at 2 pi
This commit is contained in:
Derek Witcpalek
2020-04-12 14:18:01 -04:00
parent 24ae0bd01d
commit ef26d8ffef
13 changed files with 2434 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
docker_env/gazebo/Dockerfile
View File

@@ -17,6 +17,7 @@ RUN echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc
RUN apt install -y python-rosdep
RUN rosdep init
RUN rosdep update
RUN apt-get install -y ros-melodic-pid
# user id 1000 should be the same as the host user, so that you can access files
# from inside the docker container and also on the host
RUN useradd -u 1000 rrrobot

27
rrrobot_src/src/arm_control/CMakeLists.txt
View File

@@ -149,6 +149,9 @@ include_directories(
## 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 src/arm.cpp)
add_executable(arm_movement_demo test/arm_movement_demo.cpp src/arm.cpp)
add_executable(pub_sub test/test_pub_sub.cpp)
add_executable(angles_subscriber test/angles_subscriber.cpp src/arm.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
@@ -177,6 +180,30 @@ target_link_libraries(test
ignition-math4::ignition-math4
)
target_link_libraries(arm_movement_demo
${catkin_LIBRARIES}
${kdl_parser_LIBRARIES}
${orocos_kdl_LIBRARIES}
${SDF_LIBRARIES}
ignition-math4::ignition-math4
)
target_link_libraries(pub_sub
${catkin_LIBRARIES}
${kdl_parser_LIBRARIES}
${orocos_kdl_LIBRARIES}
${SDF_LIBRARIES}
ignition-math4::ignition-math4
)
target_link_libraries(angles_subscriber
${catkin_LIBRARIES}
${kdl_parser_LIBRARIES}
${orocos_kdl_LIBRARIES}
${SDF_LIBRARIES}
ignition-math4::ignition-math4
)
#############
## Install ##
#############

27
rrrobot_src/src/arm_control/src/arm.cpp
View File

@@ -58,6 +58,15 @@ Arm::Arm(const std::string &sdf_file)
const string name(link->Get<string>("name"));
links[name].link_name = name;
// 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);
links[name].link_frame = KDL::Frame(rotation, link_position);
const sdf::ElementPtr inertial_data = link->GetElement("inertial");
float mass = inertial_data->Get<float>("mass");
links[name].mass = mass;
@@ -65,7 +74,8 @@ Arm::Arm(const std::string &sdf_file)
// Get the center of mass for this link
stringstream inertial_data_ss(inertial_data->GetElement("pose")->GetValue()->GetAsString());
inertial_data_ss >> links[name].com_location.data[0] >> links[name].com_location.data[1] >> links[name].com_location[2];
inertial_data_ss >> links[name].com_location[0] >> links[name].com_location[1] >> links[name].com_location[2];
links[name].com_location += links[name].link_frame.p;
const sdf::ElementPtr inertia = inertial_data->GetElement("inertia");
links[name].rotational_inertia = KDL::RotationalInertia(
@@ -76,15 +86,6 @@ Arm::Arm(const std::string &sdf_file)
inertia->Get<float>("ixz"),
inertia->Get<float>("iyz"));
// 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);
links[name].link_frame = KDL::Frame(rotation, link_position);
link = link->GetNextElement("link");
}
@@ -146,6 +147,7 @@ Arm::Arm(const std::string &sdf_file)
KDL::Frame prev_frame_inverse = prev_frame.Inverse();
KDL::Frame joint_relative_to_prev = prev_frame.Inverse() * joint_in_world;
KDL::Vector com_in_prev = joint_relative_to_prev * links[cur_link_name].com_location;
// // EXPERIMENTAL - update rotation axis
// KDL::Vector uncorrected_axis(0, 0, 0);
@@ -184,7 +186,7 @@ Arm::Arm(const std::string &sdf_file)
// // END EXPERIMENTAL
KDL::Joint::JointType next(links[cur_link_name].joint.getType());
KDL::RigidBodyInertia inertia(links[cur_link_name].mass, links[cur_link_name].com_location, links[cur_link_name].rotational_inertia);
KDL::RigidBodyInertia inertia(links[cur_link_name].mass, com_in_prev /*links[cur_link_name].com_location*/, links[cur_link_name].rotational_inertia);
KDL::Segment to_add(links[cur_link_name].link_name, KDL::Joint(links[cur_link_name].joint.getName(), to_set), joint_relative_to_prev, inertia);
to_set = next;
@@ -307,6 +309,9 @@ void Arm::print() const
cout << seg.getName() << endl;
cout << seg.getFrameToTip() << endl;
cout << seg.getJoint().getTypeName() << endl;
cout << "Inertial frame:" << endl;
cout << "\tmass: " << seg.getInertia().getMass() << endl;
cout << '\t' << seg.getInertia().getCOG() << endl;
cout << endl;
}
}

181
rrrobot_src/src/arm_control/src/arm_controller.cpp
View File

@@ -6,6 +6,7 @@
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/frames_io.hpp>
#include <stdio.h>
#include <vector>
#include "ros/ros.h"
#include <simulation_env/arm_command.h>
@@ -14,6 +15,7 @@
using std::cout;
using std::endl;
using std::vector;
using namespace KDL;
Arm arm("/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/model.sdf");
@@ -21,10 +23,50 @@ KDL::Chain chain = arm.getArm();
KDL::Chain simple_chain;
ros::Publisher publisher;
void enforce_torque_limits(simulation_env::arm_command &cmd, double limit = 2000.0)
{
if (cmd.shoulder_pivot_force > limit)
cmd.shoulder_pivot_force = limit;
else if (cmd.shoulder_pivot_force < -limit)
cmd.shoulder_pivot_force = -limit;
if (cmd.shoulder_joint_force > limit)
cmd.shoulder_joint_force = limit;
else if (cmd.shoulder_joint_force < -limit)
cmd.shoulder_joint_force = -limit;
if (cmd.elbow_joint_force > limit)
cmd.elbow_joint_force = limit;
else if (cmd.elbow_joint_force < -limit)
cmd.elbow_joint_force = -limit;
if (cmd.wrist_pivot_force > limit)
cmd.wrist_pivot_force = limit;
else if (cmd.wrist_pivot_force < -limit)
cmd.wrist_pivot_force = -limit;
if (cmd.wrist_joint_force > limit)
cmd.wrist_joint_force = limit;
else if (cmd.wrist_joint_force < -limit)
cmd.wrist_joint_force = -limit;
if (cmd.end_effector_pivot_force > limit)
cmd.end_effector_pivot_force = limit;
else if (cmd.end_effector_pivot_force < -limit)
cmd.end_effector_pivot_force = -limit;
}
void angle_callback(const simulation_env::arm_angles &msg)
{
// Create solver based on kinematic chain
static ChainFkSolverPos_recursive fksolver = KDL::ChainFkSolverPos_recursive(chain);
static KDL::JntArray prev_pos = KDL::JntArray(chain.getNrOfJoints());
static KDL::JntArray prev_vel = KDL::JntArray(chain.getNrOfJoints());
static double prev_time; // = ros::Time::now();
static bool first_call = true;
static simulation_env::arm_command last_command; // = {.shoulder_pivot_force = 0, .shoulder_joint_force = 0, .elbow_joint_force = 0, .wrist_pivot_force = 0, .wrist_joint_force = 0, .end_effector_pivot_force = 0};
double cur_time = ros::Time::now().toSec();
// Create joint array
unsigned int nj = chain.getNrOfJoints();
@@ -40,53 +82,122 @@ void angle_callback(const simulation_env::arm_angles &msg)
// 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)
{
// cout << nj << endl;
// cout << "shoulder_pivot: " << jointpositions(0) << endl;
// cout << "shoulder_joint: " << jointpositions(1) << endl;
// cout << "elbow_joint: " << jointpositions(2) << endl;
// cout << "wrist_pivot: " << jointpositions(3) << endl;
// cout << "wrist_joint: " << jointpositions(4) << endl;
// cout << "end_effector_pivot: " << jointpositions(5) << endl;
std::cout << cartpos.p << std::endl;
double roll, pitch, yaw;
cartpos.M.GetRPY(roll, pitch, yaw);
cout << "roll: " << roll << endl;
cout << "pitch: " << pitch << endl;
cout << "yaw: " << yaw << endl;
cout << endl;
// std::cout << cartpos << std::endl;
//printf("%s \n", "Succes, thanks KDL!");
}
else
{
//printf("%s \n", "Error: could not calculate forward kinematics :(");
}
// // Calculate forward position kinematics
// bool kinematics_status;
// kinematics_status = fksolver.JntToCart(jointpositions, cartpos);
// if (kinematics_status >= 0)
// {
// // cout << nj << endl;
// // cout << "shoulder_pivot: " << jointpositions(0) << endl;
// // cout << "shoulder_joint: " << jointpositions(1) << endl;
// // cout << "elbow_joint: " << jointpositions(2) << endl;
// // cout << "wrist_pivot: " << jointpositions(3) << endl;
// // cout << "wrist_joint: " << jointpositions(4) << endl;
// // cout << "end_effector_pivot: " << jointpositions(5) << endl;
// std::cout << cartpos.p << std::endl;
// double roll, pitch, yaw;
// cartpos.M.GetRPY(roll, pitch, yaw);
// cout << "roll: " << roll << endl;
// cout << "pitch: " << pitch << endl;
// cout << "yaw: " << yaw << endl;
// cout << endl;
// // std::cout << cartpos << std::endl;
// //printf("%s \n", "Succes, thanks KDL!");
// }
// else
// {
// //printf("%s \n", "Error: could not calculate forward kinematics :(");
// }
// try to move arm to (1, 1, 1)
double vel_default = 0.0;
KDL::JntArray vel(arm.getArm().getNrOfJoints());
KDL::JntArray accel(arm.getArm().getNrOfJoints());
// for (int joint = 0; joint < arm.getArm().getNrOfJoints(); ++joint)
// {
// // vel(joint) = 0;
// accel(joint) = 0;
// }
vector<double> last_command_values;
last_command_values.push_back(last_command.shoulder_pivot_force);
last_command_values.push_back(last_command.shoulder_joint_force);
last_command_values.push_back(last_command.elbow_joint_force);
last_command_values.push_back(last_command.wrist_pivot_force);
last_command_values.push_back(last_command.wrist_joint_force);
last_command_values.push_back(last_command.end_effector_pivot_force);
for (int joint = 0; joint < arm.getArm().getNrOfJoints(); ++joint)
{
KDL::Vector axis = arm.getArm().getSegment(joint).getJoint().JointAxis();
double inertia = KDL::dot(arm.getArm().getSegment(joint).getInertia().getRotationalInertia() * axis, axis); /*axis of rotation */
accel(joint) = (true || first_call || (inertia == 0)) ? 0.0 : last_command_values[joint] / inertia;
}
vel(0) = msg.shoulder_pivot_velocity;
vel(1) = msg.shoulder_joint_velocity;
vel(2) = msg.elbow_joint_velocity;
vel(3) = 0; //msg.wrist_pivot_velocity;
vel(4) = msg.wrist_joint_velocity;
vel(5) = 0; //msg.end_effector_pivot_velocity;
// vel(0) = 0; //(jointpositions(0) - prev_pos(0); //vel_default; //jointpositions(0) - msg.shoulder_pivot_angle;
// vel(1) = 0; //vel_default; //jointpositions(1) - msg.shoulder_joint_angle;
// vel(2) = 0; //vel_default; //jointpositions(2) - msg.elbow_joint_angle;
// vel(3) = 0; //vel_default; //jointpositions(3) - msg.wrist_pivot_angle;
// vel(4) = 0; //vel_default; //jointpositions(4) - msg.wrist_joint_angle;
// vel(5) = 0; //vel_default; //jointpositions(5) - msg.end_effector_pivot_angle;
// }
// else
// {
// for (int joint = 0; joint < arm.getArm().getNrOfJoints(); ++joint)
// {
// vel(joint) = (jointpositions(0) - prev_pos(0)) / (cur_time - prev_time);
// accel(joint) = (vel(joint) - prev_vel(joint)) / (cur_time - prev_time);
// }
// // vel(0) = (jointpositions(0) - prev_pos(0)) / (cur_time - prev_time); //vel_default; //jointpositions(0) - msg.shoulder_pivot_angle;
// // vel(1) = vel_default; //jointpositions(1) - msg.shoulder_joint_angle;
// // vel(2) = vel_default; //jointpositions(2) - msg.elbow_joint_angle;
// // vel(3) = vel_default; //jointpositions(3) - msg.wrist_pivot_angle;
// // vel(4) = vel_default; //jointpositions(4) - msg.wrist_joint_angle;
// // vel(5) = vel_default; //jointpositions(5) - msg.end_effector_pivot_angle;
// }
KDL::Wrenches ext_force(arm.getArm().getNrOfSegments());
KDL::JntArray required_force(arm.getArm().getNrOfJoints());
KDL::Frame desired(KDL::Vector(1, 1, 1));
KDL::JntArray required_positions(arm.getArm().getNrOfJoints());
bool failed = arm.calculateInverseKinematics(jointpositions, desired, required_positions);
int error_val = arm.calculateInverseDynamics(required_positions, vel, accel, ext_force, required_force);
// bool failed = arm.calculateInverseKinematics(jointpositions, desired, required_positions);
// int error_val = arm.calculateInverseDynamics(required_positions, vel, accel, ext_force, required_force);
int error_val = arm.calculateInverseDynamics(jointpositions, vel, accel, ext_force, required_force);
simulation_env::arm_command cmd;
cmd.shoulder_pivot_force = required_force(0);
cmd.shoulder_joint_force = -required_force(1);
cmd.elbow_joint_force = required_force(2);
cmd.wrist_pivot_force = required_force(3);
cmd.wrist_joint_force = required_force(4);
cmd.end_effector_pivot_force = required_force(5);
cmd.shoulder_pivot_force = 0; ///* -*/ required_force(0); // last added
cmd.shoulder_joint_force = required_force(1);
cmd.elbow_joint_force = /* -*/ required_force(2);
cmd.wrist_pivot_force = 0; ///*-*/ -required_force(3);
cmd.wrist_joint_force = /* -*/ -required_force(4);
cmd.end_effector_pivot_force = 0; ///*-*/ required_force(5);
enforce_torque_limits(cmd, 10000);
// cout << "Command force:" << endl;
// cout << "\tShoulder pivot: " << cmd.shoulder_pivot_force << endl;
// cout << "\tShoulder joint: " << cmd.shoulder_joint_force << endl;
// cout << "\tElbow joint: " << cmd.elbow_joint_force << endl;
// cout << "\tWrist pivot: " << cmd.wrist_pivot_force << endl;
// cout << "\tWrist joint: " << cmd.wrist_joint_force << endl;
// cout << "\tEnd effector pivot: " << cmd.end_effector_pivot_force << endl;
publisher.publish(cmd);
ros::spinOnce();
first_call = false;
prev_pos = jointpositions;
prev_vel = vel;
prev_time = cur_time;
last_command = cmd;
}
int main(int argc, char **argv)
@@ -94,6 +205,8 @@ int main(int argc, char **argv)
cout << "Starting arm controller..." << endl;
ros::init(argc, argv, "arm_control_test");
arm.print();
// 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))));

153
rrrobot_src/src/arm_control/test/angles_subscriber.cpp Normal file
View File

@@ -0,0 +1,153 @@
#include <std_msgs/Float64.h>
#include "ros/ros.h"
#include <ros/rate.h>
#include <arm.h>
#include <iostream>
#include <vector>
#include <cmath>
using std::cout;
using std::endl;
using std::vector;
std_msgs::Float64 shoulder_pivot_angle;
std_msgs::Float64 shoulder_joint_angle;
std_msgs::Float64 elbow_joint_angle;
std_msgs::Float64 wrist_pivot_angle;
std_msgs::Float64 wrist_joint_angle;
std_msgs::Float64 end_effector_pivot_angle;
Arm arm("/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/model.sdf");
KDL::JntArray pos(arm.getArm().getNrOfJoints());
char received_angles = 0;
void shoulder_pivot_callback(const std_msgs::Float64 &cmd)
{
// update the torque applied to each joint when a message is received
shoulder_pivot_angle = cmd;
pos(0) = cmd.data;
cout << "shoulder pivot: " << shoulder_pivot_angle.data << endl;
received_angles |= 0x01;
}
void shoulder_joint_callback(const std_msgs::Float64 &cmd)
{
// update the torque applied to each joint when a message is received
shoulder_joint_angle = cmd;
pos(1) = cmd.data;
cout << "shoulder joint: " << shoulder_joint_angle.data << endl;
received_angles |= 0x02;
}
void elbow_joint_callback(const std_msgs::Float64 &cmd)
{
// update the torque applied to each joint when a message is received
elbow_joint_angle = cmd;
pos(2) = cmd.data;
cout << "elbow joint: " << elbow_joint_angle.data << endl;
received_angles |= 0x04;
}
void wrist_pivot_callback(const std_msgs::Float64 &cmd)
{
// update the torque applied to each joint when a message is received
wrist_pivot_angle = cmd;
pos(3) = cmd.data;
cout << "wrist pivot: " << wrist_pivot_angle.data << endl;
received_angles |= 0x08;
}
void wrist_joint_callback(const std_msgs::Float64 &cmd)
{
// update the torque applied to each joint when a message is received
wrist_joint_angle = cmd;
pos(4) = cmd.data;
cout << "wrist joint: " << wrist_joint_angle.data << endl;
received_angles |= 0x10;
}
void end_effector_pivot_callback(const std_msgs::Float64 &cmd)
{
// update the torque applied to each joint when a message is received
end_effector_pivot_angle = cmd;
pos(5) = cmd.data;
cout << "end effector pivot: " << end_effector_pivot_angle.data << endl;
received_angles |= 0x20;
}
void fix_angle(double cur_pos, double &desired)
{
while (desired > M_PI)
{
desired -= 2 * M_PI;
}
while (desired < -M_PI)
{
desired += 2 * M_PI;
}
double cur_pos_truncated = cur_pos; // % (M_PI);
while (cur_pos_truncated > M_PI)
{
cur_pos_truncated -= 2 * M_PI;
}
while (cur_pos_truncated < -M_PI)
{
cur_pos_truncated += 2 * M_PI;
}
// say desired = 1.5708
// 4 cases:
// 1. cur_pos = 0.2 --> set target as cur_pos + 1.3708
// 2. cur_pos = -0.2 --> set target as cur_pos + 1.7708
// 3. cur_pos = -2.5 --> set target as cur_pos + 2.21239
// 4. cur_pos = 2.5 --> set target as cur_pos + 0.9292
if ((cur_pos_truncated - desired) < M_PI && (cur_pos_truncated - desired) > 0)
{
desired = cur_pos - (cur_pos_truncated - desired);
}
else if ((desired - cur_pos_truncated) < M_PI && (desired - cur_pos_truncated) > 0)
{
desired = cur_pos + (desired - cur_pos_truncated);
}
else if ((cur_pos_truncated - desired) > -M_PI && (cur_pos_truncated - desired) < 0)
{
desired = cur_pos - (cur_pos_truncated - desired);
}
else if ((desired - cur_pos_truncated) > -M_PI && (desired - cur_pos_truncated) < 0)
{
desired = cur_pos + (desired - cur_pos_truncated);
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "arm_control_demo");
ros::Subscriber shoulder_pivot_sub;
ros::Subscriber shoulder_joint_sub;
ros::Subscriber elbow_joint_sub;
ros::Subscriber wrist_pivot_sub;
ros::Subscriber wrist_joint_sub;
ros::Subscriber end_effector_sub;
ros::NodeHandle nh;
ros::Rate pub_rate(0.25); // publish every 10 seconds
ros::Rate quick(1);
shoulder_pivot_sub = nh.subscribe("/arm_node/arm_positions/shoulder_pivot_angle", 1000, &shoulder_pivot_callback);
shoulder_joint_sub = nh.subscribe("/arm_node/arm_positions/shoulder_joint_angle", 1000, &shoulder_joint_callback);
elbow_joint_sub = nh.subscribe("/arm_node/arm_positions/elbow_joint_angle", 1000, &elbow_joint_callback);
wrist_pivot_sub = nh.subscribe("/arm_node/arm_positions/wrist_pivot_angle", 1000, &wrist_pivot_callback);
wrist_joint_sub = nh.subscribe("/arm_node/arm_positions/wrist_joint_angle", 1000, &wrist_joint_callback);
end_effector_sub = nh.subscribe("/arm_node/arm_positions/end_effector_pivot_angle", 1000, &end_effector_pivot_callback);
ros::spin();
// while (ros::ok())
// {
// quick.sleep();
// ros::spinOnce();
// }
}

259
rrrobot_src/src/arm_control/test/arm_movement_demo.cpp Normal file
View File

@@ -0,0 +1,259 @@
#include <std_msgs/Float64.h>
#include "ros/ros.h"
#include <ros/rate.h>
#include <arm.h>
#include <iostream>
#include <vector>
#include <cmath>
using std::cout;
using std::endl;
using std::vector;
std_msgs::Float64 shoulder_pivot_angle;
std_msgs::Float64 shoulder_joint_angle;
std_msgs::Float64 elbow_joint_angle;
std_msgs::Float64 wrist_pivot_angle;
std_msgs::Float64 wrist_joint_angle;
std_msgs::Float64 end_effector_pivot_angle;
Arm arm("/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/model.sdf");
KDL::JntArray pos(arm.getArm().getNrOfJoints());
char received_angles = 0;
void shoulder_pivot_callback(const std_msgs::Float64 &cmd)
{
// update the torque applied to each joint when a message is received
shoulder_pivot_angle = cmd;
pos(0) = cmd.data;
// cout << "shoulder pivot: " << shoulder_pivot_angle.data << endl;
received_angles |= 0x01;
}
void shoulder_joint_callback(const std_msgs::Float64 &cmd)
{
// update the torque applied to each joint when a message is received
shoulder_joint_angle = cmd;
pos(1) = cmd.data;
// cout << "shoulder joint: " << shoulder_joint_angle.data << endl;
received_angles |= 0x02;
}
void elbow_joint_callback(const std_msgs::Float64 &cmd)
{
// update the torque applied to each joint when a message is received
elbow_joint_angle = cmd;
pos(2) = cmd.data;
// cout << "elbow joint: " << elbow_joint_angle.data << endl;
received_angles |= 0x04;
}
void wrist_pivot_callback(const std_msgs::Float64 &cmd)
{
// update the torque applied to each joint when a message is received
wrist_pivot_angle = cmd;
pos(3) = cmd.data;
// cout << "wrist pivot: " << wrist_pivot_angle.data << endl;
received_angles |= 0x08;
}
void wrist_joint_callback(const std_msgs::Float64 &cmd)
{
// update the torque applied to each joint when a message is received
wrist_joint_angle = cmd;
pos(4) = cmd.data;
// cout << "wrist joint: " << wrist_joint_angle.data << endl;
received_angles |= 0x10;
}
void end_effector_pivot_callback(const std_msgs::Float64 &cmd)
{
// update the torque applied to each joint when a message is received
end_effector_pivot_angle = cmd;
pos(5) = cmd.data;
// cout << "end effector pivot: " << end_effector_pivot_angle.data << endl;
received_angles |= 0x20;
}
void fix_angle(double cur_pos, double &desired)
{
while (desired > M_PI)
{
desired -= 2 * M_PI;
}
while (desired < -M_PI)
{
desired += 2 * M_PI;
}
double cur_pos_truncated = cur_pos; // % (M_PI);
while (cur_pos_truncated > M_PI)
{
cur_pos_truncated -= 2 * M_PI;
}
while (cur_pos_truncated < -M_PI)
{
cur_pos_truncated += 2 * M_PI;
}
// say desired = 1.5708
// 4 cases:
// 1. cur_pos = 0.2 --> set target as cur_pos + 1.3708
// 2. cur_pos = -0.2 --> set target as cur_pos + 1.7708
// 3. cur_pos = -2.5 --> set target as cur_pos + 2.21239
// 4. cur_pos = 2.5 --> set target as cur_pos + 0.9292
if ((cur_pos_truncated - desired) < M_PI && (cur_pos_truncated - desired) > 0)
{
desired = cur_pos - (cur_pos_truncated - desired);
}
else if ((desired - cur_pos_truncated) < M_PI && (desired - cur_pos_truncated) > 0)
{
desired = cur_pos + (desired - cur_pos_truncated);
}
else if ((cur_pos_truncated - desired) > -M_PI && (cur_pos_truncated - desired) < 0)
{
desired = cur_pos - (cur_pos_truncated - desired);
}
else if ((desired - cur_pos_truncated) > -M_PI && (desired - cur_pos_truncated) < 0)
{
desired = cur_pos + (desired - cur_pos_truncated);
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "arm_control_demo");
KDL::SetToZero(pos);
ros::Publisher shoulder_pivot_publisher;
ros::Publisher shoulder_joint_publisher;
ros::Publisher elbow_joint_publisher;
ros::Publisher wrist_pivot_publisher;
ros::Publisher wrist_joint_publisher;
ros::Publisher end_effector_pivot_publisher;
ros::Subscriber shoulder_pivot_sub;
ros::Subscriber shoulder_joint_sub;
ros::Subscriber elbow_joint_sub;
ros::Subscriber wrist_pivot_sub;
ros::Subscriber wrist_joint_sub;
ros::Subscriber end_effector_sub;
std_msgs::Float64 shoulder_pivot_target;
shoulder_pivot_target.data = 0;
std_msgs::Float64 shoulder_joint_target;
shoulder_joint_target.data = 0;
std_msgs::Float64 elbow_joint_target;
elbow_joint_target.data = 0;
std_msgs::Float64 wrist_pivot_target;
wrist_pivot_target.data = 0;
std_msgs::Float64 wrist_joint_target;
wrist_joint_target.data = 0;
std_msgs::Float64 end_effector_pivot_target;
end_effector_pivot_target.data = 0;
ros::NodeHandle nh;
ros::Rate pub_rate(0.1); // publish every 10 seconds
ros::Rate quick(1);
shoulder_pivot_publisher = nh.advertise<std_msgs::Float64>("/arm_node/shoulder_pivot_setpoint", 1000);
shoulder_joint_publisher = nh.advertise<std_msgs::Float64>("/arm_node/shoulder_joint_setpoint", 1000);
elbow_joint_publisher = nh.advertise<std_msgs::Float64>("/arm_node/elbow_joint_setpoint", 1000);
wrist_pivot_publisher = nh.advertise<std_msgs::Float64>("/arm_node/wrist_pivot_setpoint", 1000);
wrist_joint_publisher = nh.advertise<std_msgs::Float64>("/arm_node/wrist_joint_setpoint", 1000);
end_effector_pivot_publisher = nh.advertise<std_msgs::Float64>("/arm_node/end_effector_pivot_setpoint", 1000);
shoulder_pivot_sub = nh.subscribe("/arm_node/arm_positions/shoulder_pivot_angle", 1000, &shoulder_pivot_callback);
shoulder_joint_sub = nh.subscribe("/arm_node/arm_positions/shoulder_joint_angle", 1000, &shoulder_joint_callback);
elbow_joint_sub = nh.subscribe("/arm_node/arm_positions/elbow_joint_angle", 1000, &elbow_joint_callback);
wrist_pivot_sub = nh.subscribe("/arm_node/arm_positions/wrist_pivot_angle", 1000, &wrist_pivot_callback);
wrist_joint_sub = nh.subscribe("/arm_node/arm_positions/wrist_joint_angle", 1000, &wrist_joint_callback);
end_effector_sub = nh.subscribe("/arm_node/arm_positions/end_effector_pivot_angle", 1000, &end_effector_pivot_callback);
int state = 0;
KDL::JntArray required_angles(arm.getArm().getNrOfJoints());
KDL::SetToZero(required_angles);
vector<KDL::Frame> desired_positions;
// desired_positions.push_back(KDL::Frame(KDL::Vector(1, -0.5, 1)));
// desired_positions.push_back(KDL::Frame(KDL::Vector(1, -0.4, 1)));
// desired_positions.push_back(KDL::Frame(KDL::Vector(1, -0.3, 1)));
// desired_positions.push_back(KDL::Frame(KDL::Vector(1, -0.2, 1)));
// desired_positions.push_back(KDL::Frame(KDL::Vector(1, -0.1, 1)));
// desired_positions.push_back(KDL::Frame(KDL::Vector(1, 0.0, 1)));
// desired_positions.push_back(KDL::Frame(KDL::Vector(1, 0.1, 1)));
// desired_positions.push_back(KDL::Frame(KDL::Vector(1, 0.2, 1)));
// desired_positions.push_back(KDL::Frame(KDL::Vector(1, 0.3, 1)));
// desired_positions.push_back(KDL::Frame(KDL::Vector(1, 0.4, 1)));
// desired_positions.push_back(KDL::Frame(KDL::Vector(1, 0.5, 1)));
desired_positions.push_back(KDL::Frame(KDL::Vector(1, 1, 1)));
desired_positions.push_back(KDL::Frame(KDL::Vector(1, -1, 1)));
desired_positions.push_back(KDL::Frame(KDL::Vector(1, -1, 2)));
desired_positions.push_back(KDL::Frame(KDL::Vector(1, 1, 3)));
while (ros::ok())
{
// ros::spinOnce();
// ros::spinOnce();
// quick.sleep();
// pos(0) = shoulder_pivot_angle.data;
// pos(1) = shoulder_joint_angle.data;
// pos(2) = elbow_joint_angle.data;
// pos(3) = wrist_pivot_angle.data;
// pos(4) = wrist_joint_angle.data;
// pos(5) = end_effector_pivot_angle.data;
// if (state) //fabs(shoulder_joint_target.data) < 0.001)
// {
arm.calculateInverseKinematics(pos, desired_positions[state] /*KDL::Frame(KDL::Vector(1, 0, 0))*/, required_angles);
//shoulder_joint_target.data = 0.7;
// }
// else
// {
// arm.calculateInverseKinematics(pos, KDL::Frame(KDL::Vector(-1, -1, 1)), required_angles);
// //shoulder_joint_target.data = 0.0;
// }
cout << desired_positions[state].p.x() << " " << desired_positions[state].p.y() << " " << desired_positions[state].p.z() << endl;
cout << "\t" << required_angles(0) << "\t" << required_angles(1) << "\t" << required_angles(2) << "\t" << required_angles(3) << "\t" << required_angles(4) << "\t" << required_angles(5) << endl;
cout << endl;
for (int idx = 0; idx < arm.getArm().getNrOfJoints(); ++idx)
{
// cout << "Cur: " << pos(idx) << "\tRequired: " << required_angles(idx) << endl;
fix_angle(pos(idx), required_angles(idx));
// cout << "\tPost fix: Required: " << required_angles(idx) << endl;
}
shoulder_pivot_target.data = required_angles(0);
shoulder_joint_target.data = required_angles(1);
elbow_joint_target.data = required_angles(2);
wrist_pivot_target.data = required_angles(3);
wrist_joint_target.data = required_angles(4);
end_effector_pivot_target.data = required_angles(5);
// cout << "Received angles: " << (int)received_angles << endl;
// if (received_angles == 0b00111111)
// {
shoulder_pivot_publisher.publish(shoulder_pivot_target);
shoulder_joint_publisher.publish(shoulder_joint_target);
elbow_joint_publisher.publish(elbow_joint_target);
wrist_pivot_publisher.publish(wrist_pivot_target);
wrist_joint_publisher.publish(wrist_joint_target);
end_effector_pivot_publisher.publish(end_effector_pivot_target);
state = (state + 1) % desired_positions.size();
// }
// else
// {
// ros::spinOnce();
// }
pub_rate.sleep();
ros::spinOnce();
}
}

48
rrrobot_src/src/arm_control/test/test.cpp
View File

@@ -13,12 +13,16 @@
#include <iostream>
#include <arm.h>
#include <fstream>
#include <string>
#include <vector>
using namespace KDL;
using std::cin;
using std::cout;
using std::endl;
using std::ofstream;
using std::string;
using std::vector;
int main(int argc, char **argv)
{
@@ -155,4 +159,48 @@ int main(int argc, char **argv)
{
cout << "Failed to find required torques, error #" << error_val << endl;
}
// record required torques for a variety of joint positions
KDL::JntArray pos(arm.getArm().getNrOfJoints());
for (int joint = 0; joint < arm.getArm().getNrOfJoints(); ++joint)
{
pos(joint) = 0;
}
ofstream f("required_" + std::to_string(which_joint) + "_torque.txt");
for (double angle = -3.14159; angle < 3.14159; angle += 0.1)
{
pos(which_joint) = angle;
arm.calculateInverseDynamics(pos, vel, accel, ext_force, required_force);
f << angle << "," << required_force(0) << "," << required_force(1) << "," << required_force(2) << "," << required_force(3) << "," << required_force(4) << "," << required_force(5) << "\n";
}
// KDL::JntArray pos(arm.getArm().getNrOfJoints());
KDL::JntArray required_angles(arm.getArm().getNrOfJoints());
KDL::SetToZero(pos);
KDL::SetToZero(required_angles);
vector<KDL::Frame> desired_positions;
desired_positions.push_back(KDL::Frame(KDL::Vector(1, -0.5, 1)));
desired_positions.push_back(KDL::Frame(KDL::Vector(1, -0.4, 1)));
desired_positions.push_back(KDL::Frame(KDL::Vector(1, -0.3, 1)));
desired_positions.push_back(KDL::Frame(KDL::Vector(1, -0.2, 1)));
desired_positions.push_back(KDL::Frame(KDL::Vector(1, -0.1, 1)));
desired_positions.push_back(KDL::Frame(KDL::Vector(1, 0.0, 1)));
desired_positions.push_back(KDL::Frame(KDL::Vector(1, 0.1, 1)));
desired_positions.push_back(KDL::Frame(KDL::Vector(1, 0.2, 1)));
desired_positions.push_back(KDL::Frame(KDL::Vector(1, 0.3, 1)));
desired_positions.push_back(KDL::Frame(KDL::Vector(1, 0.4, 1)));
desired_positions.push_back(KDL::Frame(KDL::Vector(1, 0.5, 1)));
ofstream ik_f("inverse_kinematics.txt");
for (int state = 0; state < desired_positions.size(); ++state)
{
arm.calculateInverseKinematics(pos, desired_positions[state], required_angles);
ik_f << desired_positions[state].p.x() << "," << desired_positions[state].p.y() << "," << desired_positions[state].p.z() << ",";
ik_f << required_angles(0) << "," << required_angles(1) << "," << required_angles(2) << "," << required_angles(3) << "," << required_angles(4) << "," << required_angles(5) << "\n";
}
}

97
rrrobot_src/src/arm_control/test/test_pub_sub.cpp Normal file
View File

@@ -0,0 +1,97 @@
#include "ros/ros.h"
#include "std_msgs/Float64.h"
#include <iostream>
using std::cout;
using std::endl;
void msg_callback(const std_msgs::Float64 &cmd)
{
// update the torque applied to each joint when a message is received
cout << "received: " << cmd.data << endl;
}
/**
* This tutorial demonstrates simple sending of messages over the ROS system.
*/
int main(int argc, char **argv)
{
/**
* The ros::init() function needs to see argc and argv so that it can perform
* any ROS arguments and name remapping that were provided at the command line.
* For programmatic remappings you can use a different version of init() which takes
* remappings directly, but for most command-line programs, passing argc and argv is
* the easiest way to do it. The third argument to init() is the name of the node.
*
* You must call one of the versions of ros::init() before using any other
* part of the ROS system.
*/
ros::init(argc, argv, "talker");
/**
* NodeHandle is the main access point to communications with the ROS system.
* The first NodeHandle constructed will fully initialize this node, and the last
* NodeHandle destructed will close down the node.
*/
ros::NodeHandle n;
/**
* The advertise() function is how you tell ROS that you want to
* publish on a given topic name. This invokes a call to the ROS
* master node, which keeps a registry of who is publishing and who
* is subscribing. After this advertise() call is made, the master
* node will notify anyone who is trying to subscribe to this topic name,
* and they will in turn negotiate a peer-to-peer connection with this
* node. advertise() returns a Publisher object which allows you to
* publish messages on that topic through a call to publish(). Once
* all copies of the returned Publisher object are destroyed, the topic
* will be automatically unadvertised.
*
* The second parameter to advertise() is the size of the message queue
* used for publishing messages. If messages are published more quickly
* than we can send them, the number here specifies how many messages to
* buffer up before throwing some away.
*/
ros::Publisher pub = n.advertise<std_msgs::Float64>("chatter", 1000);
ros::Subscriber sub = n.subscribe("chatter", 1000, &msg_callback);
ros::Rate loop_rate(10);
/**
* A count of how many messages we have sent. This is used to create
* a unique string for each message.
*/
int count = 0;
double data = -5e-5; //.1254987465413251e-5;
while (ros::ok())
{
/**
* This is a message object. You stuff it with data, and then publish it.
*/
std_msgs::Float64 msg;
msg.data = data;
cout << "Publishing: " << data << '\t' << msg.data << endl;
/**
* The publish() function is how you send messages. The parameter
* is the message object. The type of this object must agree with the type
* given as a template parameter to the advertise<>() call, as was done
* in the constructor above.
*/
pub.publish(msg);
ros::spinOnce();
loop_rate.sleep();
++count;
data += 1e-5;
if (data > 5.0)
{
data = -5.1254987465413251;
}
}
return 0;
}

26
rrrobot_src/src/gazebo_models/fanuc_robotic_arm/model.sdf
View File

@@ -17,7 +17,7 @@
</inertial>
<self_collide>1</self_collide>
<kinematic>0</kinematic>
<gravity>1</gravity>
<gravity>0</gravity>
<visual name='visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
@@ -116,7 +116,7 @@
</inertial>
<self_collide>1</self_collide>
<kinematic>0</kinematic>
<gravity>1</gravity>
<gravity>0</gravity>
<visual name='visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
@@ -215,7 +215,7 @@
</inertial>
<self_collide>1</self_collide>
<kinematic>0</kinematic>
<gravity>1</gravity>
<gravity>0</gravity>
<visual name='visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
@@ -314,7 +314,7 @@
</inertial>
<self_collide>1</self_collide>
<kinematic>0</kinematic>
<gravity>1</gravity>
<gravity>0</gravity>
<visual name='visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
@@ -413,7 +413,7 @@
</inertial>
<self_collide>1</self_collide>
<kinematic>0</kinematic>
<gravity>1</gravity>
<gravity>0</gravity>
<visual name='visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
@@ -512,7 +512,7 @@
</inertial>
<self_collide>1</self_collide>
<kinematic>0</kinematic>
<gravity>1</gravity>
<gravity>0</gravity>
<visual name='visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
@@ -611,7 +611,7 @@
</inertial>
<self_collide>1</self_collide>
<kinematic>0</kinematic>
<gravity>1</gravity>
<gravity>0</gravity>
<visual name='visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
@@ -704,7 +704,7 @@
<limit>
<lower>-1.79769e+308</lower>
<upper>1.79769e+308</upper>
<effort>-2000</effort>
<effort>-20000000</effort>
<velocity>-1</velocity>
</limit>
<dynamics>
@@ -737,7 +737,7 @@
<limit>
<lower>-1.79769e+308</lower>
<upper>1.79769e+308</upper>
<effort>-2000</effort>
<effort>-20000000</effort>
<velocity>-1</velocity>
</limit>
<dynamics>
@@ -770,7 +770,7 @@
<limit>
<lower>-1.79769e+308</lower>
<upper>1.79769e+308</upper>
<effort>-2000</effort>
<effort>-20000000</effort>
<velocity>-1</velocity>
</limit>
<dynamics>
@@ -803,7 +803,7 @@
<limit>
<lower>-1.79769e+308</lower>
<upper>1.79769e+308</upper>
<effort>-2000</effort>
<effort>-20000000</effort>
<velocity>-1</velocity>
</limit>
<dynamics>
@@ -836,7 +836,7 @@
<limit>
<lower>-1.79769e+308</lower>
<upper>1.79769e+308</upper>
<effort>-2000</effort>
<effort>-20000000</effort>
<velocity>-1</velocity>
</limit>
<dynamics>
@@ -869,7 +869,7 @@
<limit>
<lower>-1.79769e+308</lower>
<upper>1.79769e+308</upper>
<effort>-2000</effort>
<effort>-20000000</effort>
<velocity>-1</velocity>
</limit>
<dynamics>

7
rrrobot_src/src/simulation_env/msg/arm_angles.msg
View File

@@ -4,3 +4,10 @@ float32 elbow_joint_angle
float32 wrist_pivot_angle
float32 wrist_joint_angle
float32 end_effector_pivot_angle
float32 shoulder_pivot_velocity
float32 shoulder_joint_velocity
float32 elbow_joint_velocity
float32 wrist_pivot_velocity
float32 wrist_joint_velocity
float32 end_effector_pivot_velocity

92
rrrobot_src/src/simulation_env/src/arm_angle_sensor_plugin.cpp
View File

@@ -2,9 +2,11 @@
#include <gazebo/physics/physics.hh>
#include <memory>
#include <iostream>
#include <cmath>
#include "ros/ros.h"
#include "simulation_env/arm_angles.h"
#include <std_msgs/Float64.h>
using std::cout;
using std::endl;
@@ -29,7 +31,33 @@ public:
}
nh.reset(new ros::NodeHandle("arm_node"));
publisher = nh->advertise<simulation_env::arm_angles>("arm_positions", 1);
// publisher = nh->advertise<simulation_env::arm_angles>("arm_positions", 1000);
shoulder_pivot_pub = nh->advertise<std_msgs::Float64>("/arm_node/arm_positions/shoulder_pivot_angle", 1);
shoulder_joint_pub = nh->advertise<std_msgs::Float64>("/arm_node/arm_positions/shoulder_joint_angle", 1);
elbow_joint_pub = nh->advertise<std_msgs::Float64>("/arm_node/arm_positions/elbow_joint_angle", 1);
wrist_pivot_pub = nh->advertise<std_msgs::Float64>("/arm_node/arm_positions/wrist_pivot_angle", 1);
wrist_joint_pub = nh->advertise<std_msgs::Float64>("/arm_node/arm_positions/wrist_joint_angle", 1);
end_effector_pivot_pub = nh->advertise<std_msgs::Float64>("/arm_node/arm_positions/end_effector_pivot_angle", 1);
std_msgs::Float64 shoulder_pivot_angle;
shoulder_pivot_angle.data = model->GetJoint("shoulder_pivot")->Position(); //msg.shoulder_pivot_angle;
std_msgs::Float64 shoulder_joint_angle;
shoulder_joint_angle.data = model->GetJoint("shoulder_joint")->Position(); //msg.shoulder_joint_angle;
std_msgs::Float64 elbow_joint_angle;
elbow_joint_angle.data = model->GetJoint("elbow_joint")->Position(); //msg.elbow_joint_angle;
std_msgs::Float64 wrist_pivot_angle;
wrist_pivot_angle.data = model->GetJoint("wrist_pivot")->Position(); //msg.wrist_pivot_angle;
std_msgs::Float64 wrist_joint_angle;
wrist_joint_angle.data = model->GetJoint("wrist_joint")->Position(); //msg.wrist_joint_angle;
std_msgs::Float64 end_effector_pivot_angle;
end_effector_pivot_angle.data = model->GetJoint("end_effector_pivot")->Position(); //msg.end_effector_pivot_angle;
// cout << "shoulder_pivot: " << shoulder_pivot_angle.data << endl;
// cout << "shoulder_joint: " << shoulder_joint_angle.data << endl;
// cout << "elbow_joint: " << elbow_joint_angle.data << endl;
// cout << "wrist_pivot: " << wrist_pivot_angle.data << endl;
// cout << "wrist_joint: " << wrist_joint_angle.data << endl;
// cout << "end_effector_pivot: " << end_effector_pivot_angle.data << endl;
}
private:
@@ -37,16 +65,53 @@ private:
{
simulation_env::arm_angles msg;
// read in the joint angle for each joint in the arm
msg.shoulder_pivot_angle = model->GetJoint("shoulder_pivot")->Position();
msg.shoulder_joint_angle = model->GetJoint("shoulder_joint")->Position();
msg.elbow_joint_angle = model->GetJoint("elbow_joint")->Position();
msg.wrist_pivot_angle = model->GetJoint("wrist_pivot")->Position();
msg.wrist_joint_angle = model->GetJoint("wrist_joint")->Position();
msg.end_effector_pivot_angle = model->GetJoint("end_effector_pivot")->Position();
// // read in the joint angle for each joint in the arm
// msg.shoulder_pivot_angle = model->GetJoint("shoulder_pivot")->Position();
// msg.shoulder_joint_angle = -model->GetJoint("shoulder_joint")->Position();
// msg.elbow_joint_angle = -model->GetJoint("elbow_joint")->Position();
// msg.wrist_pivot_angle = model->GetJoint("wrist_pivot")->Position();
// msg.wrist_joint_angle = model->GetJoint("wrist_joint")->Position() + M_PI_2;
// msg.end_effector_pivot_angle = model->GetJoint("end_effector_pivot")->Position();
// publish the updated sensor measurements
publisher.publish(msg);
// // read in the angular velocity for each joint
// msg.shoulder_pivot_velocity = model->GetJoint("shoulder_pivot")->GetVelocity(0);
// msg.shoulder_joint_velocity = -model->GetJoint("shoulder_joint")->GetVelocity(0);
// msg.elbow_joint_velocity = -model->GetJoint("elbow_joint")->GetVelocity(0);
// msg.wrist_pivot_velocity = model->GetJoint("wrist_pivot")->GetVelocity(0);
// msg.wrist_joint_velocity = model->GetJoint("wrist_joint")->GetVelocity(0);
// msg.end_effector_pivot_velocity = model->GetJoint("end_effector_pivot")->GetVelocity(0);
// // publish the updated sensor measurements
// publisher.publish(msg);
// ros::spinOnce();
std_msgs::Float64 shoulder_pivot_angle;
shoulder_pivot_angle.data = model->GetJoint("shoulder_pivot")->Position(); //msg.shoulder_pivot_angle;
std_msgs::Float64 shoulder_joint_angle;
shoulder_joint_angle.data = model->GetJoint("shoulder_joint")->Position(); //msg.shoulder_joint_angle;
std_msgs::Float64 elbow_joint_angle;
elbow_joint_angle.data = model->GetJoint("elbow_joint")->Position(); //msg.elbow_joint_angle;
std_msgs::Float64 wrist_pivot_angle;
wrist_pivot_angle.data = model->GetJoint("wrist_pivot")->Position(); //msg.wrist_pivot_angle;
std_msgs::Float64 wrist_joint_angle;
wrist_joint_angle.data = model->GetJoint("wrist_joint")->Position(); //msg.wrist_joint_angle;
std_msgs::Float64 end_effector_pivot_angle;
end_effector_pivot_angle.data = model->GetJoint("end_effector_pivot")->Position(); //msg.end_effector_pivot_angle;
// cout << "Publishing: " << shoulder_pivot_angle.data << '\t' << shoulder_joint_angle.data << '\t' << elbow_joint_angle.data << '\t' << wrist_pivot_angle.data << '\t' << wrist_joint_angle.data << '\t' << end_effector_pivot_angle.data << endl;
shoulder_pivot_pub.publish(shoulder_pivot_angle);
// ros::spinOnce();
shoulder_joint_pub.publish(shoulder_joint_angle);
// ros::spinOnce();
elbow_joint_pub.publish(elbow_joint_angle);
// ros::spinOnce();
wrist_pivot_pub.publish(wrist_pivot_angle);
// ros::spinOnce();
wrist_joint_pub.publish(wrist_joint_angle);
// ros::spinOnce();
end_effector_pivot_pub.publish(end_effector_pivot_angle);
// ros::spinOnce();
// make sure the message gets published
ros::spinOnce();
@@ -56,6 +121,13 @@ private:
event::ConnectionPtr updateConnection;
std::unique_ptr<ros::NodeHandle> nh;
ros::Publisher publisher;
ros::Publisher shoulder_pivot_pub;
ros::Publisher shoulder_joint_pub;
ros::Publisher elbow_joint_pub;
ros::Publisher wrist_pivot_pub;
ros::Publisher wrist_joint_pub;
ros::Publisher end_effector_pivot_pub;
};
GZ_REGISTER_MODEL_PLUGIN(JointAngle)
} // namespace gazebo

76
rrrobot_src/src/simulation_env/src/arm_motors.cpp
View File

@@ -5,6 +5,7 @@
#include "ros/ros.h"
#include "simulation_env/arm_command.h"
#include <std_msgs/Float64.h>
using std::cout;
using std::endl;
@@ -14,15 +15,63 @@ namespace gazebo
class ArmControl : public ModelPlugin
{
public:
void shoulder_pivot_callback(const std_msgs::Float64 &cmd)
{
// update the torque applied to each joint when a message is received
model->GetJoint("shoulder_pivot")->SetForce(0, cmd.data);
ros::spinOnce();
}
void shoulder_joint_callback(const std_msgs::Float64 &cmd)
{
// update the torque applied to each joint when a message is received
model->GetJoint("shoulder_joint")->SetForce(0, cmd.data);
ros::spinOnce();
}
void elbow_joint_callback(const std_msgs::Float64 &cmd)
{
// update the torque applied to each joint when a message is received
model->GetJoint("elbow_joint")->SetForce(0, cmd.data);
ros::spinOnce();
}
void wrist_pivot_callback(const std_msgs::Float64 &cmd)
{
// update the torque applied to each joint when a message is received
model->GetJoint("wrist_pivot")->SetForce(0, cmd.data);
ros::spinOnce();
}
void wrist_joint_callback(const std_msgs::Float64 &cmd)
{
// update the torque applied to each joint when a message is received
model->GetJoint("wrist_joint")->SetForce(0, cmd.data);
ros::spinOnce();
}
void end_effector_pivot_callback(const std_msgs::Float64 &cmd)
{
// update the torque applied to each joint when a message is received
model->GetJoint("end_effector_pivot")->SetForce(0, cmd.data);
ros::spinOnce();
}
void arm_command_callback(const simulation_env::arm_command &cmd)
{
// update the torque applied to each joint when a message is received
model->GetJoint("shoulder_pivot")->SetForce(0, cmd.shoulder_pivot_force);
model->GetJoint("shoulder_joint")->SetForce(0, cmd.shoulder_joint_force);
model->GetJoint("elbow_joint")->SetForce(0, cmd.elbow_joint_force);
model->GetJoint("wrist_pivot")->SetForce(0, cmd.wrist_pivot_force);
model->GetJoint("wrist_joint")->SetForce(0, cmd.wrist_joint_force);
model->GetJoint("end_effector_pivot")->SetForce(0, cmd.end_effector_pivot_force);
// model->GetJoint("shoulder_pivot")->SetForce(0, cmd.shoulder_pivot_force);
// model->GetJoint("shoulder_joint")->SetForce(0, -cmd.shoulder_joint_force);
// model->GetJoint("elbow_joint")->SetForce(0, cmd.elbow_joint_force);
// model->GetJoint("wrist_pivot")->SetForce(0, cmd.wrist_pivot_force);
// model->GetJoint("wrist_joint")->SetForce(0, cmd.wrist_joint_force);
// model->GetJoint("end_effector_pivot")->SetForce(0, cmd.end_effector_pivot_force);
ros::spinOnce();
}
@@ -42,12 +91,27 @@ public:
nh.reset(new ros::NodeHandle("arm_node"));
subscriber = nh->subscribe("arm_commands", 1000, &ArmControl::arm_command_callback, this);
shoulder_pivot_sub = nh->subscribe("/arm_node/arm_commands/shoulder_pivot_torque", 1000, &ArmControl::shoulder_pivot_callback, this);
shoulder_joint_sub = nh->subscribe("/arm_node/arm_commands/shoulder_joint_torque", 1000, &ArmControl::shoulder_joint_callback, this);
elbow_joint_sub = nh->subscribe("/arm_node/arm_commands/elbow_joint_torque", 1000, &ArmControl::elbow_joint_callback, this);
wrist_pivot_sub = nh->subscribe("/arm_node/arm_commands/wrist_pivot_torque", 1000, &ArmControl::wrist_pivot_callback, this);
wrist_joint_sub = nh->subscribe("/arm_node/arm_commands/wrist_joint_torque", 1000, &ArmControl::wrist_joint_callback, this);
end_effector_sub = nh->subscribe("/arm_node/arm_commands/end_effector_pivot_torque", 1000, &ArmControl::end_effector_pivot_callback, this);
cout << "Subscribed to all torque channels" << endl;
}
private:
physics::ModelPtr model;
std::unique_ptr<ros::NodeHandle> nh;
ros::Subscriber subscriber;
ros::Subscriber shoulder_pivot_sub;
ros::Subscriber shoulder_joint_sub;
ros::Subscriber elbow_joint_sub;
ros::Subscriber wrist_pivot_sub;
ros::Subscriber wrist_joint_sub;
ros::Subscriber end_effector_sub;
};
GZ_REGISTER_MODEL_PLUGIN(ArmControl)
} // namespace gazebo

1514
rrrobot_src/world/rrrobot_setpoint.world Normal file
View File

File diff suppressed because it is too large Load Diff