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

Modified arm controller node to handle all arm control. It receives desired end effector poses and actuates the arm. Also includes some clean up.

Browse Source
This commit is contained in:
Derek Witcpalek
2020-04-15 15:52:49 -04:00
parent ef26d8ffef
commit ba6901d32a
9 changed files with 331 additions and 591 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
rrrobot_src/build.sh
View File

@@ -5,3 +5,4 @@ catkin_make &&
catkin_make install &&
source devel/setup.bash
export GAZEBO_RESOURCE_PATH=$(pwd)/world:$GAZEBO_RESOURCE_PATH

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

@@ -10,6 +10,7 @@ add_compile_options(-std=c++11;-g3)
find_package(catkin REQUIRED COMPONENTS
roscpp
std_msgs
geometry_msgs
)
## System dependencies are found with CMake's conventions

48
rrrobot_src/src/arm_control/include/arm_control/arm.h
View File

@@ -23,47 +23,6 @@ public:
*/
Arm(const std::string &sdf_file);
/*
* Gets the center of mass of the remaining arm, starting from the
* link in index (ie the center of mass of links l[i]-l[n])
*/
KDL::Vector getCOM(size_t index);
/*
* Gets the center of mass of the remaining arm, starting from the
* link named 'link_name'
*/
KDL::Vector getCOM(const std::string &link_name);
/*
* Gets the inertia of the remaining arm about the joint in index. This
* treats the preceding segments as if they are stationary and calculates
* the inertia of the remaining arm.
*/
float getInertia(size_t index);
/*
* Gets the inertia of the remaining arm about the joint named 'joint_name'. This
* treats the preceding segments as if they are stationary and calculates
* the inertia of the remaining arm.
*/
float getInertia(const std::string &joint_name);
/*
* This returns the mass of the remaining links, starting from (and including)
* the link at index
*/
float getSupportedMass(size_t index);
/*
* This returns the mass of the remaining links, starting from (and including)
* the link named 'link_name'
*/
float getSupportedMass(const std::string &link_name);
/*
* This returns the torques required to get the arm moving with the desired
* angular acceleration.
*/
std::vector<float> getRequiredTorques(/*std::vector<float> theta_des, std::vector<float> theta_dot_des, */ std::vector<float> theta_double_dot_des);
/*
* Using the provided joint positions, this calculates the position of the end of the
* arm. Returns whether it was successful
@@ -85,11 +44,4 @@ private:
std::unique_ptr<KDL::ChainFkSolverPos_recursive> fksolver;
std::unique_ptr<KDL::ChainIkSolverPos_LMA> iksolver;
std::unique_ptr<KDL::ChainIdSolver_RNE> idsolver;
/*
* i_com: the inertia about the center of mass of this link
* mass: the mass of this link
* distance: the distance from the center of mass to the new axis
*/
float parallelAxisTheorem(float i_com, float mass, float distance);
};

123
rrrobot_src/src/arm_control/launch/arm.launch Normal file
View File

@@ -0,0 +1,123 @@
<launch>
<node name="shoulder_pivot_pid" pkg="pid" type="controller" output="screen" >
<param name="Kp" value="450.0" />
<param name="Ki" value="5.0" />
<param name="Kd" value="600.0" />
<param name="upper_limit" value="10000" />
<param name="lower_limit" value="-10000" />
<param name="windup_limit" value="1000" />
<param name="cutoff_frequency" value="20" />
<param name="max_loop_frequency" value="105.0" />
<param name="min_loop_frequency" value="95.0" />
<param name="setpoint_timeout" value="-1.0" />
<param name="topic_from_controller" value="/arm_node/arm_commands/shoulder_pivot_torque" />
<param name="topic_from_plant" value="/arm_node/arm_positions/shoulder_pivot_angle" />
<param name="setpoint_topic" value="/arm_node/shoulder_pivot_setpoint" />
</node>
<node name="shoulder_joint_pid" pkg="pid" type="controller" output="screen" >
<param name="Kp" value="1200.0" />
<param name="Ki" value="40.0" />
<param name="Kd" value="1400.0" />
<param name="upper_limit" value="10000" />
<param name="lower_limit" value="-10000" />
<param name="windup_limit" value="1000" />
<param name="cutoff_frequency" value="20" />
<param name="max_loop_frequency" value="105.0" />
<param name="min_loop_frequency" value="95.0" />
<param name="setpoint_timeout" value="-1.0" />
<param name="topic_from_controller" value="/arm_node/arm_commands/shoulder_joint_torque" />
<param name="topic_from_plant" value="/arm_node/arm_positions/shoulder_joint_angle" />
<param name="setpoint_topic" value="/arm_node/shoulder_joint_setpoint" />
</node>
<node name="elbow_joint_pid" pkg="pid" type="controller" output="screen" >
<param name="Kp" value="400.0" />
<param name="Ki" value="15.0" />
<param name="Kd" value="400.0" />
<param name="upper_limit" value="10000" />
<param name="lower_limit" value="-10000" />
<param name="windup_limit" value="1000" />
<param name="cutoff_frequency" value="20" />
<param name="max_loop_frequency" value="105.0" />
<param name="min_loop_frequency" value="95.0" />
<param name="setpoint_timeout" value="-1.0" />
<param name="topic_from_controller" value="/arm_node/arm_commands/elbow_joint_torque" />
<param name="topic_from_plant" value="/arm_node/arm_positions/elbow_joint_angle" />
<param name="setpoint_topic" value="/arm_node/elbow_joint_setpoint" />
</node>
<node name="wrist_pivot_pid" pkg="pid" type="controller" output="screen" >
<param name="Kp" value="3.0" />
<param name="Ki" value="0.5" />
<param name="Kd" value="1.0" />
<param name="upper_limit" value="1000" />
<param name="lower_limit" value="-1000" />
<param name="windup_limit" value="100" />
<param name="cutoff_frequency" value="20" />
<param name="max_loop_frequency" value="105.0" />
<param name="min_loop_frequency" value="95.0" />
<param name="setpoint_timeout" value="-1.0" />
<param name="topic_from_controller" value="/arm_node/arm_commands/wrist_pivot_torque" />
<param name="topic_from_plant" value="/arm_node/arm_positions/wrist_pivot_angle" />
<param name="setpoint_topic" value="/arm_node/wrist_pivot_setpoint" />
</node>
<node name="wrist_joint_pid" pkg="pid" type="controller" output="screen" >
<param name="Kp" value="2.0" />
<param name="Ki" value="0.5" />
<param name="Kd" value="1.0" />
<param name="upper_limit" value="1000" />
<param name="lower_limit" value="-1000" />
<param name="windup_limit" value="100" />
<param name="cutoff_frequency" value="20" />
<param name="max_loop_frequency" value="105.0" />
<param name="min_loop_frequency" value="95.0" />
<param name="setpoint_timeout" value="-1.0" />
<param name="topic_from_controller" value="/arm_node/arm_commands/wrist_joint_torque" />
<param name="topic_from_plant" value="/arm_node/arm_positions/wrist_joint_angle" />
<param name="setpoint_topic" value="/arm_node/wrist_joint_setpoint" />
</node>
<node name="end_effector_pivot_pid" pkg="pid" type="controller" output="screen" >
<param name="Kp" value="0.0" />
<param name="Ki" value="0.0" />
<param name="Kd" value="0.0" />
<param name="upper_limit" value="1000" />
<param name="lower_limit" value="-1000" />
<param name="windup_limit" value="100" />
<param name="cutoff_frequency" value="20" />
<param name="max_loop_frequency" value="105.0" />
<param name="min_loop_frequency" value="95.0" />
<param name="setpoint_timeout" value="-1.0" />
<param name="topic_from_controller" value="/arm_node/arm_commands/end_effector_pivot_torque" />
<param name="topic_from_plant" value="/arm_node/arm_positions/end_effector_pivot_angle" />
<param name="setpoint_topic" value="/arm_node/end_effector_pivot_setpoint" />
</node>
<!-- <node name="servo_sim_node" pkg="pid" type="plant_sim" output="screen" >
<param name="plant_order" value="2" />
</node> -->
<node name="arm_controller" pkg="arm_control" type="arm_control_node" output="screen" />
<node name="arm_movement_demo" pkg="arm_control" type="arm_movement_demo" output="screen" />
<!-- rqt_plot is a resource hog, so if you're seeing high CPU usage, don't launch rqt_plot -->
<!-- node name="rqt_plot" pkg="rqt_plot" type="rqt_plot"
args="/control_effort/data /state/data /setpoint/data" -->
<node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" />
<!--node name="rqt_robot_monitor" pkg="rqt_robot_monitor" type="rqt_robot_monitor" -->
<!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="rrrobot_setpoint.world"/> <!--Note: the world_name is with respect to GAZEBO_RESOURCE_PATH environmental variable -->
<arg name="paused" value="true"/>
<arg name="use_sim_time" value="true"/>
<arg name="gui" value="true"/>
<arg name="recording" value="false"/>
<arg name="debug" value="false"/>
</include>
</launch>

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

@@ -53,8 +53,6 @@ Arm::Arm(const std::string &sdf_file)
sdf::ElementPtr link = model->GetElement("link");
while (link)
{
// cout << "Link: " << link->Get<string>("name") << endl;
const string name(link->Get<string>("name"));
links[name].link_name = name;
@@ -70,7 +68,6 @@ Arm::Arm(const std::string &sdf_file)
const sdf::ElementPtr inertial_data = link->GetElement("inertial");
float mass = inertial_data->Get<float>("mass");
links[name].mass = mass;
// cout << "Mass: " << mass << endl;
// Get the center of mass for this link
stringstream inertial_data_ss(inertial_data->GetElement("pose")->GetValue()->GetAsString());
@@ -120,7 +117,7 @@ Arm::Arm(const std::string &sdf_file)
else if (axis_num == 2)
joint_type = KDL::Joint::JointType::RotZ;
links[child].joint = KDL::Joint(name, joint_type); // correct
links[child].joint = KDL::Joint(name, joint_type);
stringstream pose_string(joint->GetElement("pose")->GetValue()->GetAsString());
float x, y, z;
@@ -129,7 +126,7 @@ Arm::Arm(const std::string &sdf_file)
KDL::Rotation frame_rotation = KDL::Rotation::RPY(roll, pitch, yaw);
KDL::Vector frame_location(x, y, z);
links[child].joint_frame = KDL::Frame(frame_rotation, frame_location); // incorrect --> this is actually the
links[child].joint_frame = KDL::Frame(frame_rotation, frame_location);
joint = joint->GetNextElement("joint");
}
@@ -148,60 +145,12 @@ Arm::Arm(const std::string &sdf_file)
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);
// if (links[cur_link_name].joint.getType() == KDL::Joint::JointType::RotX)
// {
// uncorrected_axis = KDL::Vector(1, 0, 0);
// }
// else if (links[cur_link_name].joint.getType() == KDL::Joint::JointType::RotY)
// {
// uncorrected_axis = KDL::Vector(0, 1, 0);
// }
// else if (links[cur_link_name].joint.getType() == KDL::Joint::JointType::RotZ)
// {
// uncorrected_axis = KDL::Vector(0, 0, 1);
// }
// KDL::Vector corrected_axis = joint_relative_to_prev.M * uncorrected_axis;
// double x = fabs(corrected_axis.x());
// double y = fabs(corrected_axis.y());
// double z = fabs(corrected_axis.z());
// const string &name(links[cur_link_name].joint.getName());
// if (x > y and x > z)
// {
// links[cur_link_name].joint = KDL::Joint(name, KDL::Joint::JointType::RotX);
// }
// else if (y > x and y > z)
// {
// links[cur_link_name].joint = KDL::Joint(name, KDL::Joint::JointType::RotY);
// }
// else if (z > x and z > y)
// {
// links[cur_link_name].joint = KDL::Joint(name, KDL::Joint::JointType::RotZ);
// }
// // END EXPERIMENTAL
KDL::Joint::JointType next(links[cur_link_name].joint.getType());
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;
// cout << cur_link_name << endl;
double roll, pitch, yaw;
// cout << " joint in world coordinates:" << endl;
// cout << joint_in_world << endl;
// cout << " joint in previous joint's frame:" << endl;
// cout << joint_relative_to_prev << endl;
// cout << " previous frame inverse:" << endl;
// cout << prev_frame_inverse << endl;
// cout << "Joint type: " << links[cur_link_name].joint.getTypeName() << "\n\n"
// << endl;
arm.addSegment(to_add);
prev_frame = joint_in_world;
@@ -215,64 +164,6 @@ Arm::Arm(const std::string &sdf_file)
idsolver.reset(new KDL::ChainIdSolver_RNE(arm, KDL::Vector(0, 0, -9.81)));
}
/*
* Gets the center of mass of the remaining arm, starting from the
* link in index (ie the center of mass of links l[i]-l[n])
*/
KDL::Vector Arm::getCOM(size_t index)
{
}
/*
* Gets the center of mass of the remaining arm, starting from the
* link named 'link_name'
*/
KDL::Vector Arm::getCOM(const std::string &link_name)
{
}
/*
* Gets the inertia of the remaining arm about the joint in index. This
* treats the preceding segments as if they are stationary and calculates
* the inertia of the remaining arm.
*/
float Arm::getInertia(size_t index)
{
}
/*
* Gets the inertia of the remaining arm about the joint named 'joint_name'. This
* treats the preceding segments as if they are stationary and calculates
* the inertia of the remaining arm.
*/
float Arm::getInertia(const std::string &joint_name)
{
}
/*
* This returns the mass of the remaining links, starting from (and including)
* the link at index
*/
float Arm::getSupportedMass(size_t index)
{
}
/*
* This returns the mass of the remaining links, starting from (and including)
* the link named 'link_name'
*/
float Arm::getSupportedMass(const std::string &link_name)
{
}
/*
* This returns the torques required to get the arm moving with the desired
* angular acceleration.
*/
std::vector<float> Arm::getRequiredTorques(/*std::vector<float> theta_des, std::vector<float> theta_dot_des, */ std::vector<float> theta_double_dot_des)
{
}
/*
* Using the provided joint positions, this calculates the pose of the end of the
* arm. Returns whether it was successful
@@ -289,11 +180,6 @@ bool Arm::calculateInverseKinematics(const KDL::JntArray &cur_configuration, con
int Arm::calculateInverseDynamics(const KDL::JntArray &pos, const KDL::JntArray &vel, const KDL::JntArray &accel, const KDL::Wrenches &f_external, KDL::JntArray &required_torque)
{
// cout << pos.rows() << endl;
// cout << vel.rows() << endl;
// cout << accel.rows() << endl;
// cout << f_external.size() << endl;
// cout << required_torque.rows() << endl;
return idsolver->CartToJnt(pos, vel, accel, f_external, required_torque);
}
@@ -315,12 +201,3 @@ void Arm::print() const
cout << endl;
}
}
/*
* i_com: the inertia about the center of mass of this link
* mass: the mass of this link
* distance: the distance from the center of mass to the new axis
*/
float Arm::parallelAxisTheorem(float i_com, float mass, float distance)
{
}

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

@@ -1,6 +1,9 @@
#include <arm.h>
#include <iostream>
#include <geometry_msgs/Pose.h>
#include <std_msgs/Float64.h>
#include <kdl/chain.hpp>
#include <kdl/chainfksolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
@@ -20,226 +23,106 @@ using namespace KDL;
Arm arm("/home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/model.sdf");
KDL::Chain chain = arm.getArm();
KDL::Chain simple_chain;
ros::Publisher publisher;
KDL::JntArray pos(arm.getArm().getNrOfJoints());
void enforce_torque_limits(simulation_env::arm_command &cmd, double limit = 2000.0)
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;
void shoulder_pivot_callback(const std_msgs::Float64 &cmd)
{
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;
pos(0) = cmd.data;
}
void angle_callback(const simulation_env::arm_angles &msg)
void shoulder_joint_callback(const std_msgs::Float64 &cmd)
{
// 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};
pos(1) = cmd.data;
}
double cur_time = ros::Time::now().toSec();
void elbow_joint_callback(const std_msgs::Float64 &cmd)
{
pos(2) = cmd.data;
}
// Create joint array
unsigned int nj = chain.getNrOfJoints();
KDL::JntArray jointpositions = KDL::JntArray(nj);
void wrist_pivot_callback(const std_msgs::Float64 &cmd)
{
pos(3) = cmd.data;
}
jointpositions(0) = msg.shoulder_pivot_angle;
jointpositions(1) = msg.shoulder_joint_angle;
jointpositions(2) = msg.elbow_joint_angle;
jointpositions(3) = msg.wrist_pivot_angle;
jointpositions(4) = msg.wrist_joint_angle;
jointpositions(5) = msg.end_effector_pivot_angle;
void wrist_joint_callback(const std_msgs::Float64 &cmd)
{
pos(4) = cmd.data;
}
// Create the frame that will contain the results
KDL::Frame cartpos;
void end_effector_pivot_callback(const std_msgs::Float64 &cmd)
{
pos(5) = cmd.data;
}
// // 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;
void fixAngles(const KDL::JntArray &current_angles, KDL::JntArray &desired_angles)
{
// TODO: go clockwise or counter-clockwise depending on which is shorter
}
// 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;
void endEffectorPoseTargetCallback(const geometry_msgs::Pose &msg)
{
KDL::JntArray required_angles(arm.getArm().getNrOfJoints());
KDL::SetToZero(required_angles);
// // std::cout << cartpos << std::endl;
// //printf("%s \n", "Succes, thanks KDL!");
// }
// else
// {
// //printf("%s \n", "Error: could not calculate forward kinematics :(");
// }
KDL::Frame desired(KDL::Rotation::Quaternion(msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w),
KDL::Vector(msg.position.x, msg.position.y, msg.position.z));
// 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());
arm.calculateInverseKinematics(pos, desired, required_angles);
// for (int joint = 0; joint < arm.getArm().getNrOfJoints(); ++joint)
// {
// // vel(joint) = 0;
// accel(joint) = 0;
// }
fixAngles(pos, required_angles);
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);
std_msgs::Float64 shoulder_pivot_setpoint;
shoulder_pivot_setpoint.data = required_angles(0);
std_msgs::Float64 shoulder_joint_setpoint;
shoulder_joint_setpoint.data = required_angles(1);
std_msgs::Float64 elbow_joint_setpoint;
elbow_joint_setpoint.data = required_angles(2);
std_msgs::Float64 wrist_pivot_setpoint;
wrist_pivot_setpoint.data = required_angles(3);
std_msgs::Float64 wrist_joint_setpoint;
wrist_joint_setpoint.data = required_angles(4);
std_msgs::Float64 end_effector_setpoint;
end_effector_setpoint.data = required_angles(5);
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);
int error_val = arm.calculateInverseDynamics(jointpositions, vel, accel, ext_force, required_force);
simulation_env::arm_command cmd;
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;
shoulder_pivot_publisher.publish(shoulder_pivot_setpoint);
shoulder_joint_publisher.publish(shoulder_joint_setpoint);
elbow_joint_publisher.publish(elbow_joint_setpoint);
wrist_pivot_publisher.publish(wrist_pivot_setpoint);
wrist_joint_publisher.publish(wrist_joint_setpoint);
end_effector_pivot_publisher.publish(end_effector_setpoint);
}
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))));
ros::init(argc, argv, "arm_control_node");
ros::NodeHandle nh;
publisher = nh.advertise<simulation_env::arm_command>("/arm_node/arm_commands", 1000);
ros::Subscriber sub = nh.subscribe("/arm_node/arm_positions", 1000, angle_callback);
ros::Subscriber shoulder_pivot_sub = nh.subscribe("/arm_node/arm_positions/shoulder_pivot_angle", 1000, &shoulder_pivot_callback);
ros::Subscriber shoulder_joint_sub = nh.subscribe("/arm_node/arm_positions/shoulder_joint_angle", 1000, &shoulder_joint_callback);
ros::Subscriber elbow_joint_sub = nh.subscribe("/arm_node/arm_positions/elbow_joint_angle", 1000, &elbow_joint_callback);
ros::Subscriber wrist_pivot_sub = nh.subscribe("/arm_node/arm_positions/wrist_pivot_angle", 1000, &wrist_pivot_callback);
ros::Subscriber wrist_joint_sub = nh.subscribe("/arm_node/arm_positions/wrist_joint_angle", 1000, &wrist_joint_callback);
ros::Subscriber end_effector_sub = nh.subscribe("/arm_node/arm_positions/end_effector_pivot_angle", 1000, &end_effector_pivot_callback);
cout << "0 positions for each segment" << endl;
cout << "Base:" << endl;
cout << chain.getSegment(0).pose(0.0) << endl;
cout << "Link_1:" << endl;
cout << chain.getSegment(0).pose(0.0) * chain.getSegment(1).pose(0.0) << endl;
cout << "Link_2:" << endl;
cout << chain.getSegment(0).pose(0) * chain.getSegment(1).pose(0.0) * chain.getSegment(2).pose(0.0) << endl;
cout << "Link_3:" << endl;
cout << chain.getSegment(0).pose(0) * chain.getSegment(1).pose(0.0) * chain.getSegment(2).pose(0.0) * chain.getSegment(3).pose(0.0) << endl;
cout << "Link_4:" << endl;
cout << chain.getSegment(0).pose(0) * chain.getSegment(1).pose(0.0) * chain.getSegment(2).pose(0.0) * chain.getSegment(3).pose(0.0) * chain.getSegment(4).pose(0.0) << endl;
cout << "Link_5:" << endl;
cout << chain.getSegment(0).pose(0) * chain.getSegment(1).pose(0.0) * chain.getSegment(2).pose(0.0) * chain.getSegment(3).pose(0.0) * chain.getSegment(4).pose(0.0) * chain.getSegment(5).pose(0.0) << endl;
cout << "Link_6:" << endl;
cout << chain.getSegment(0).pose(0) * chain.getSegment(1).pose(0.0) * chain.getSegment(2).pose(0.0) * chain.getSegment(3).pose(0.0) * chain.getSegment(4).pose(0.0) * chain.getSegment(5).pose(0.0) * chain.getSegment(6).pose(0.0) << endl;
ros::Subscriber end_effector_pose_setpoint_sub = nh.subscribe("/arm_node/arm_pose_setpoint", 1000, &endEffectorPoseTargetCallback);
// Assign some values to the joint positions
// for (unsigned int i = 0; i < nj; i++)
// {
// float myinput;
// // printf("Enter the position of joint %i: ", i);
// // scanf("%e", &myinput);
// jointpositions(i) = (double)0.0;
// }
KDL::SetToZero(pos);
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);
ros::spin();
cout << "Arm controller finished." << endl;
}

234
rrrobot_src/src/arm_control/test/arm_movement_demo.cpp
View File

@@ -1,4 +1,4 @@
#include <std_msgs/Float64.h>
#include <geometry_msgs/Pose.h>
#include "ros/ros.h"
#include <ros/rate.h>
@@ -12,248 +12,36 @@ 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);
ros::Publisher end_effector_setpoint_publisher = nh.advertise<geometry_msgs::Pose>("/arm_node/arm_pose_setpoint", 1000);
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::Rotation::RPY(-2.35619, 0.7853, 0.0), 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();
const KDL::Frame &cur_setpoint = desired_positions[state];
geometry_msgs::Pose msg;
msg.position.x = cur_setpoint.p.x();
msg.position.y = cur_setpoint.p.y();
msg.position.z = cur_setpoint.p.z();
cur_setpoint.M.GetQuaternion(msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w);
// 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);
end_effector_setpoint_publisher.publish(msg);
ros::spinOnce();
state = (state + 1) % desired_positions.size();
// }
// else
// {
// ros::spinOnce();
// }
pub_rate.sleep();
ros::spinOnce();
}
}

11
rrrobot_src/src/gazebo_models/unit_box/model.config Normal file
View File

@@ -0,0 +1,11 @@
<?xml version="1.0" ?>
<model>
<name>unit_box</name>
<version>1.0</version>
<sdf version="1.6">model.sdf</sdf>
<author>
<name></name>
<email></email>
</author>
<description></description>
</model>

104
rrrobot_src/src/gazebo_models/unit_box/model.sdf Normal file
View File

@@ -0,0 +1,104 @@
<?xml version='1.0'?>
<sdf version='1.6'>
<model name='unit_box'>
<link name='link'>
<inertial>
<mass>0.208095</mass>
<inertia>
<ixx>0.0180922</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0180922</iyy>
<iyz>0</iyz>
<izz>0.0346825</izz>
</inertia>
<pose frame=''>0 0 0 0 -0 0</pose>
</inertial>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
<pose frame=''>0 0 0 0 -0 0</pose>
<gravity>1</gravity>
<visual name='visual'>
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
<material>
<script>
<name>Gazebo/Grey</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
<shader type='pixel'>
<normal_map>__default__</normal_map>
</shader>
<ambient>1 0 0 1</ambient>
<diffuse>0.7 0.7 0.7 1</diffuse>
<specular>0.01 0.01 0.01 1</specular>
<emissive>0 0 0 1</emissive>
</material>
<pose frame=''>0 0 0 0 -0 0</pose>
<transparency>0</transparency>
<cast_shadows>1</cast_shadows>
</visual>
<collision name='collision'>
<laser_retro>0</laser_retro>
<max_contacts>10</max_contacts>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<box>
<size>0.05 0.05 0.05</size>
</box>
</geometry>
<surface>
<friction>
<ode>
<mu>1</mu>
<mu2>1</mu2>
<fdir1>0 0 0</fdir1>
<slip1>0</slip1>
<slip2>0</slip2>
</ode>
<torsional>
<coefficient>1</coefficient>
<patch_radius>0</patch_radius>
<surface_radius>0</surface_radius>
<use_patch_radius>1</use_patch_radius>
<ode>
<slip>0</slip>
</ode>
</torsional>
</friction>
<bounce>
<restitution_coefficient>0</restitution_coefficient>
<threshold>1e+06</threshold>
</bounce>
<contact>
<collide_without_contact>0</collide_without_contact>
<collide_without_contact_bitmask>1</collide_without_contact_bitmask>
<collide_bitmask>1</collide_bitmask>
<ode>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e+13</kp>
<kd>1</kd>
<max_vel>0.01</max_vel>
<min_depth>0</min_depth>
</ode>
<bullet>
<split_impulse>1</split_impulse>
<split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e+13</kp>
<kd>1</kd>
</bullet>
</contact>
</surface>
</collision>
</link>
<static>0</static>
<allow_auto_disable>1</allow_auto_disable>
</model>
</sdf>