00001 #include "ros/ros.h"
00002 #include "kinematics_msgs/GetPositionIK.h"
00003 #include <kdl_parser/kdl_parser.hpp>
00004 #include <geometry_msgs/Pose.h>
00005 #include <pr2_controllers_msgs/JointTrajectoryControllerState.h>
00006 #include <tf_conversions/tf_kdl.h>
00007 #include <sensor_msgs/JointState.h>
00008 #include <cob_srvs/Trigger.h>
00009 #include <trajectory_msgs/JointTrajectory.h>
00010 #include <geometry_msgs/Twist.h>
00011 #include <nav_msgs/Odometry.h>
00012 #include <visualization_msgs/Marker.h>
00013
00014 #include <kdl/chainfksolver.hpp>
00015 #include <kdl/chainfksolverpos_recursive.hpp>
00016 #include <kdl/chainiksolvervel_pinv.hpp>
00017 #include <kdl/chainiksolverpos_nr.hpp>
00018 #include <kdl/frames_io.hpp>
00019 #include <kdl/jntarray.hpp>
00020 #include <kdl/path_line.hpp>
00021 #include <kdl/path_circle.hpp>
00022 #include <kdl/frames.hpp>
00023 #include <kdl/rotational_interpolation_sa.hpp>
00024 #include <kdl/velocityprofile_trap.hpp>
00025 #include <kdl/trajectory_segment.hpp>
00026
00027
00028
00029
00030 using namespace std;
00031 using namespace KDL;
00032 KDL::Chain chain;
00033 KDL::Chain chain_base_arm0;
00034 KDL::JntArray VirtualQ;
00035 KDL::JntArray q;
00036 bool started = false;
00037 KDL::Twist extTwist;
00038 KDL::Twist baseTwist;
00039 ros::Time last;
00040 double lastgradx = 0.0;
00041 double lastgrady = 0.0;
00042
00043 ChainFkSolverPos_recursive * fksolver1;
00044 ChainIkSolverVel_pinv * iksolver1v;
00045 ChainIkSolverPos_NR * iksolverpos;
00046
00047
00048 bool RunSyncMM = false;
00049 ros::Publisher arm_pub_;
00050 ros::Publisher base_pub_;
00051
00052
00053
00054 JntArray parseJointStates(std::vector<std::string> names, std::vector<double> positions)
00055 {
00056 JntArray q_temp(7);
00057 int count = 0;
00058 for(int i = 0; i < names.size(); i++)
00059 {
00060 if(strncmp(names[i].c_str(), "arm_", 4) == 0)
00061 {
00062 q_temp(count) = positions[i];
00063 count++;
00064 }
00065 }
00066 if(!started)
00067 {
00068 VirtualQ = q_temp;
00069 started = true;
00070 last = ros::Time::now();
00071 ROS_INFO("Starting up controller with first configuration");
00072 }
00073 return q_temp;
00074 }
00075
00076 void cartTwistCallback(const geometry_msgs::Twist::ConstPtr& msg)
00077 {
00078 extTwist.vel.x(msg->linear.x);
00079 extTwist.vel.y(msg->linear.y);
00080 extTwist.vel.z(msg->linear.z);
00081
00082 extTwist.rot.x(msg->angular.x);
00083 extTwist.rot.x(msg->angular.y);
00084 extTwist.rot.x(msg->angular.z);
00085 }
00086
00087 void baseTwistCallback(const nav_msgs::Odometry::ConstPtr& msg)
00088 {
00089 if(RunSyncMM)
00090 {
00091 double vx = msg->twist.twist.linear.x;
00092 double vy = msg->twist.twist.linear.y;
00093 double omega = msg->twist.twist.angular.z;
00094
00095 Frame F_ist;
00096 fksolver1->JntToCart(q, F_ist, -1);
00097
00098 KDL::Vector twist_Trans = (KDL::Rotation::EulerZYX(-omega, 0.0, 0.0) * F_ist.p) - F_ist.p;
00099
00100
00101
00102
00103 KDL::Rotation T_Rot_AB_RB(-0.683013,0.258819,-0.683013,
00104 0.183013,0.965926,0.183013,
00105 0.707107,0.00000,-0.707107);
00106 Frame F_armbase;
00107 ChainFkSolverPos_recursive fksolver_base_armv0(chain_base_arm0);
00108
00109 fksolver_base_armv0.JntToCart(NULL, F_armbase, -1);
00110
00111
00112
00113 KDL::Frame T_AB_RB(F_armbase.M);
00114
00115 KDL::Vector twist_Rot = KDL::Vector(0,0, -omega);
00116
00117
00118
00119
00120 KDL::Twist RotTwist(twist_Trans, KDL::Vector(0,0,0));
00121
00122 baseTwist.vel.x(-vx);
00123 baseTwist.vel.y(-vy);
00124
00125 baseTwist += RotTwist;
00126 }
00127 }
00128
00129 KDL::Twist getTwist(KDL::Frame F_ist)
00130 {
00131 KDL::Twist zero;
00132 if(!RunSyncMM)
00133 return zero;
00134 else
00135 {
00136 return baseTwist;
00137 }
00138 }
00139
00140
00141
00142 void Manipulability_potentialfield_lookup()
00143 {
00144 double gradx;
00145 double grady;
00146
00147 Frame F_ist;
00148 fksolver1->JntToCart(q, F_ist, -1);
00149
00150
00151 gradx = 0.420 - F_ist.p.x();
00152 grady = -0.711655 - F_ist.p.y();
00153
00154 gradx = (gradx * gradx * gradx)/2;
00155 grady = (grady * grady * grady)/2;
00156
00157
00158 double maxvel = 0.2;
00159 if(gradx > maxvel)
00160 gradx = maxvel;
00161 if(gradx < -maxvel)
00162 gradx = -maxvel;
00163 if(grady > maxvel)
00164 grady = maxvel;
00165 if(grady < -maxvel)
00166 grady = -maxvel;
00167 if(abs(gradx) < 0.02)
00168 gradx = 0;
00169 if(abs(grady) < 0.02)
00170 grady = 0;
00171
00172
00173 double maxacc = 0.01;
00174 if((gradx-lastgradx) > maxacc)
00175 gradx = lastgradx + maxacc;
00176
00177 lastgradx = gradx;
00178 lastgrady = grady;
00179
00180 geometry_msgs::Twist cmd;
00181 cmd.linear.x = gradx;
00182 cmd.linear.y = grady;
00183 if(RunSyncMM)
00184 base_pub_.publish(cmd);
00185 }
00186
00187 bool SyncMMTrigger(cob_srvs::Trigger::Request& request, cob_srvs::Trigger::Response& response)
00188 {
00189 if(RunSyncMM)
00190 RunSyncMM = false;
00191 else
00192 {
00193 started = false;
00194 RunSyncMM = true;
00195 }
00196 return true;
00197 }
00198
00199 void sendVel(JntArray q_t, JntArray q_dot)
00200 {
00201 ros::Time now = ros::Time::now();
00202 double dt = now.toSec() - last.toSec();
00203 last = now;
00204 double horizon = 3.0*dt;
00205
00206
00207 trajectory_msgs::JointTrajectory traj;
00208 traj.header.stamp = ros::Time::now()+ros::Duration(0.01);
00209 traj.joint_names.push_back("arm_1_joint");
00210 traj.joint_names.push_back("arm_2_joint");
00211 traj.joint_names.push_back("arm_3_joint");
00212 traj.joint_names.push_back("arm_4_joint");
00213 traj.joint_names.push_back("arm_5_joint");
00214 traj.joint_names.push_back("arm_6_joint");
00215 traj.joint_names.push_back("arm_7_joint");
00216
00217 traj.points.resize(1);
00218 bool nonzero = false;
00219 for(int i = 0; i < 7; i++)
00220 {
00221 if(q_dot(i) != 0.0)
00222 {
00223 traj.points[0].positions.push_back(VirtualQ(i) + q_dot(i)*horizon);
00224 traj.points[0].velocities.push_back(q_dot(i));
00225 VirtualQ(i) += q_dot(i)*dt;
00226 nonzero = true;
00227 }
00228 }
00229 traj.points[0].time_from_start = ros::Duration(horizon);
00230 if(nonzero)
00231 arm_pub_.publish(traj);
00232 }
00233
00234 void controllerStateCallback(const sensor_msgs::JointState::ConstPtr& msg)
00235 {
00236 if(RunSyncMM)
00237 {
00238 unsigned int nj = chain.getNrOfJoints();
00239 std::vector<std::string> names = msg->name;
00240 std::vector<double> positions = msg->position;
00241 q = parseJointStates(names,positions);
00242
00243
00244 JntArray q_out(7);
00245 Frame F_ist;
00246 fksolver1->JntToCart(q, F_ist, -1);
00247 KDL::Twist combined_twist = extTwist + getTwist(F_ist);
00248 int ret = iksolver1v->CartToJnt(q, combined_twist, q_out);
00249 if(ret >= 0)
00250 {
00251 sendVel(q, q_out);
00252 std::cout << q_out(0) << " " << q_out(1) << " " << q_out(2) << " " << q_out(3) << " " << q_out(4) << " " << q_out(5) << " " << q_out(6) << "\n";
00253 }
00254 else
00255 std::cout << "Something went wrong" << "\n";
00256 }
00257 }
00258
00259 int main(int argc, char **argv)
00260 {
00261 ros::init(argc, argv, "cob_mm_controller");
00262 ros::NodeHandle n;
00263 KDL::Tree my_tree;
00264 ros::NodeHandle node;
00265 std::string robot_desc_string;
00266 node.param("/robot_description", robot_desc_string, string());
00267 if (!kdl_parser::treeFromString(robot_desc_string, my_tree)){
00268 ROS_ERROR("Failed to construct kdl tree");
00269 return false;
00270 }
00271 my_tree.getChain("base_link","arm_7_link", chain);
00272 my_tree.getChain("base_link","arm_0_link", chain_base_arm0);
00273
00274 fksolver1 = new ChainFkSolverPos_recursive(chain);
00275 iksolver1v = new ChainIkSolverVel_pinv(chain);
00276
00277
00278 ros::Subscriber sub = n.subscribe("/joint_states", 1, controllerStateCallback);
00279 ros::Subscriber cart_vel_sub = n.subscribe("/arm_controller/cart/command", 1, cartTwistCallback);
00280 ros::Subscriber plat_odom_sub = n.subscribe("/base_controller/odometry", 1, baseTwistCallback);
00281 arm_pub_ = n.advertise<trajectory_msgs::JointTrajectory>("/arm_controller/command",1);
00282 base_pub_ = n.advertise<geometry_msgs::Twist>("/base_controller/command",1);
00283
00284 ros::ServiceServer serv = n.advertiseService("/mm/run", SyncMMTrigger);
00285 ROS_INFO("Running cartesian velocity controller.");
00286 ros::spin();
00287
00288 return 0;
00289 }