diff --git a/rrrobot_src/build.sh b/rrrobot_src/build.sh index 6b4669d..912ee85 100755 --- a/rrrobot_src/build.sh +++ b/rrrobot_src/build.sh @@ -5,3 +5,4 @@ catkin_make && catkin_make install && source devel/setup.bash +export GAZEBO_RESOURCE_PATH=$(pwd)/world:$GAZEBO_RESOURCE_PATH diff --git a/rrrobot_src/src/arm_control/CMakeLists.txt b/rrrobot_src/src/arm_control/CMakeLists.txt index c6a915a..3ac02e5 100644 --- a/rrrobot_src/src/arm_control/CMakeLists.txt +++ b/rrrobot_src/src/arm_control/CMakeLists.txt @@ -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 diff --git a/rrrobot_src/src/arm_control/include/arm_control/arm.h b/rrrobot_src/src/arm_control/include/arm_control/arm.h index 5b2cad9..a54abfb 100644 --- a/rrrobot_src/src/arm_control/include/arm_control/arm.h +++ b/rrrobot_src/src/arm_control/include/arm_control/arm.h @@ -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 getRequiredTorques(/*std::vector theta_des, std::vector theta_dot_des, */ std::vector 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 fksolver; std::unique_ptr iksolver; std::unique_ptr 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); }; \ No newline at end of file diff --git a/rrrobot_src/src/arm_control/launch/arm.launch b/rrrobot_src/src/arm_control/launch/arm.launch new file mode 100644 index 0000000..e153bf4 --- /dev/null +++ b/rrrobot_src/src/arm_control/launch/arm.launch @@ -0,0 +1,123 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rrrobot_src/src/arm_control/src/arm.cpp b/rrrobot_src/src/arm_control/src/arm.cpp index d24aaa4..3a8a6f3 100644 --- a/rrrobot_src/src/arm_control/src/arm.cpp +++ b/rrrobot_src/src/arm_control/src/arm.cpp @@ -53,8 +53,6 @@ Arm::Arm(const std::string &sdf_file) sdf::ElementPtr link = model->GetElement("link"); while (link) { - // cout << "Link: " << link->Get("name") << endl; - const string name(link->Get("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("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 Arm::getRequiredTorques(/*std::vector theta_des, std::vector theta_dot_des, */ std::vector 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); } @@ -314,13 +200,4 @@ void Arm::print() const cout << '\t' << seg.getInertia().getCOG() << endl; 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) -{ } \ No newline at end of file diff --git a/rrrobot_src/src/arm_control/src/arm_controller.cpp b/rrrobot_src/src/arm_control/src/arm_controller.cpp index a54e74f..5fdd735 100644 --- a/rrrobot_src/src/arm_control/src/arm_controller.cpp +++ b/rrrobot_src/src/arm_control/src/arm_controller.cpp @@ -1,6 +1,9 @@ #include #include +#include +#include + #include #include #include @@ -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 ¤t_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 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("/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("/arm_node/shoulder_pivot_setpoint", 1000); + shoulder_joint_publisher = nh.advertise("/arm_node/shoulder_joint_setpoint", 1000); + elbow_joint_publisher = nh.advertise("/arm_node/elbow_joint_setpoint", 1000); + wrist_pivot_publisher = nh.advertise("/arm_node/wrist_pivot_setpoint", 1000); + wrist_joint_publisher = nh.advertise("/arm_node/wrist_joint_setpoint", 1000); + end_effector_pivot_publisher = nh.advertise("/arm_node/end_effector_pivot_setpoint", 1000); ros::spin(); - cout << "Arm controller finished." << endl; } \ No newline at end of file diff --git a/rrrobot_src/src/arm_control/test/arm_movement_demo.cpp b/rrrobot_src/src/arm_control/test/arm_movement_demo.cpp index e35cef5..4c493ef 100644 --- a/rrrobot_src/src/arm_control/test/arm_movement_demo.cpp +++ b/rrrobot_src/src/arm_control/test/arm_movement_demo.cpp @@ -1,4 +1,4 @@ -#include +#include #include "ros/ros.h" #include @@ -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("/arm_node/shoulder_pivot_setpoint", 1000); - shoulder_joint_publisher = nh.advertise("/arm_node/shoulder_joint_setpoint", 1000); - elbow_joint_publisher = nh.advertise("/arm_node/elbow_joint_setpoint", 1000); - wrist_pivot_publisher = nh.advertise("/arm_node/wrist_pivot_setpoint", 1000); - wrist_joint_publisher = nh.advertise("/arm_node/wrist_joint_setpoint", 1000); - end_effector_pivot_publisher = nh.advertise("/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("/arm_node/arm_pose_setpoint", 1000); int state = 0; - KDL::JntArray required_angles(arm.getArm().getNrOfJoints()); - KDL::SetToZero(required_angles); vector 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(); } } \ No newline at end of file diff --git a/rrrobot_src/src/gazebo_models/unit_box/model.config b/rrrobot_src/src/gazebo_models/unit_box/model.config new file mode 100644 index 0000000..2a703a6 --- /dev/null +++ b/rrrobot_src/src/gazebo_models/unit_box/model.config @@ -0,0 +1,11 @@ + + + unit_box + 1.0 + model.sdf + + + + + + diff --git a/rrrobot_src/src/gazebo_models/unit_box/model.sdf b/rrrobot_src/src/gazebo_models/unit_box/model.sdf new file mode 100644 index 0000000..1c42598 --- /dev/null +++ b/rrrobot_src/src/gazebo_models/unit_box/model.sdf @@ -0,0 +1,104 @@ + + + + + + 0.208095 + + 0.0180922 + 0 + 0 + 0.0180922 + 0 + 0.0346825 + + 0 0 0 0 -0 0 + + 0 + 0 + 0 + 0 0 0 0 -0 0 + 1 + + + + 0.05 0.05 0.05 + + + + + + __default__ + + 1 0 0 1 + 0.7 0.7 0.7 1 + 0.01 0.01 0.01 1 + 0 0 0 1 + + 0 0 0 0 -0 0 + 0 + 1 + + + 0 + 10 + 0 0 0 0 -0 0 + + + 0.05 0.05 0.05 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + 0 + 1 + +