00001
00028 #include <ros/ros.h>
00029
00030 #include <cob_lookat_controller/cob_lookat_controller.h>
00031
00032 #include <kdl_conversions/kdl_msg.h>
00033 #include <tf/transform_datatypes.h>
00034
00035 #include <urdf/model.h>
00036 #include <std_srvs/Empty.h>
00037
00038
00039 void CobLookatController::initialize()
00040 {
00042 XmlRpc::XmlRpcValue lookat_jn_param;
00043 if (nh_.hasParam("lookat_joint_names"))
00044 { nh_.getParam("lookat_joint_names", lookat_jn_param); }
00045 else
00046 { ROS_ERROR("Parameter lookat_joint_names not set"); }
00047
00048 lookat_dof_ = lookat_jn_param.size();
00049 for(unsigned int i=0; i<lookat_dof_; i++)
00050 { lookat_joints_.push_back((std::string)lookat_jn_param[i]); }
00051
00052 XmlRpc::XmlRpcValue chain_jn_param;
00053 if (nh_.hasParam("chain_joint_names"))
00054 { nh_.getParam("chain_joint_names", chain_jn_param); }
00055 else
00056 { ROS_ERROR("Parameter chain_joint_names not set"); }
00057
00058 chain_dof_ = chain_jn_param.size();
00059 for(unsigned int i=0; i<chain_dof_; i++)
00060 { chain_joints_.push_back((std::string)chain_jn_param[i]); }
00061
00062 total_dof_ = chain_dof_ + lookat_dof_;
00063 total_joints_ = chain_joints_;
00064 total_joints_.insert(total_joints_.end(), lookat_joints_.begin(), lookat_joints_.end());
00065
00067 urdf::Model model;
00068 if (!model.initParam("/robot_description")){
00069 ROS_ERROR("Failed to parse urdf file");
00070 }
00071 ROS_INFO("Successfully parsed urdf file");
00072
00073 chain_limits_vel_.push_back(model.getJoint("torso_1_joint")->limits->velocity);
00074 chain_limits_vel_.push_back(model.getJoint("torso_2_joint")->limits->velocity);
00075 chain_limits_vel_.push_back(model.getJoint("torso_3_joint")->limits->velocity);
00076
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087 if (nh_.hasParam("base_link"))
00088 {
00089 nh_.getParam("base_link", chain_base_);
00090 }
00091 if (nh_.hasParam("tip_link"))
00092 {
00093 nh_.getParam("tip_link", chain_tip_);
00094 }
00095
00096
00098 KDL::Tree my_tree;
00099 std::string robot_desc_string;
00100 nh_.param("/robot_description", robot_desc_string, std::string());
00101 if (!kdl_parser::treeFromString(robot_desc_string, my_tree)){
00102 ROS_ERROR("Failed to construct kdl tree");
00103 return;
00104 }
00105 my_tree.getChain(chain_base_, chain_tip_, chain_);
00106 if(chain_.getNrOfJoints() == 0)
00107 {
00108 ROS_ERROR("Failed to initialize kinematic chain");
00109 return;
00110 }
00111
00112
00114
00115 p_iksolver_vel_wdls_ = new KDL::ChainIkSolverVel_wdls(chain_, 0.001, 5);
00116
00118 Eigen::MatrixXd Mq(7,7);
00119 for (int i=0; i<7; i++) {
00120 for (int j=0; j<7; j++) {
00121 Mq(i,j) = 0;
00122 }
00123 }
00124 std::cout << Mq << std::endl;
00125 Mq(0,0) = 1;
00126 Mq(1,1) = 1;
00127 Mq(2,2) = 1;
00128 Mq(3,3) = 1;
00129 Mq(4,4) = 1;
00130 Mq(5,5) = 1;
00131 Mq(6,6) = 1;
00132 std::cout << Mq << std::endl;
00133
00135 p_iksolver_vel_wdls_->setWeightJS(Mq);
00136
00137
00138
00139
00140
00142
00144
00145
00146
00147
00148 if (nh_.hasParam("chain_vel_pub_topic"))
00149 { nh_.getParam("chain_vel_pub_topic", chain_vel_pub_topic_); }
00150 else
00151 { ROS_ERROR("Parameter chain_vel_pub_topic not set"); }
00152
00154 jointstate_sub = nh_.subscribe("/joint_states", 1, &CobLookatController::jointstate_cb, this);
00155
00156 twist_sub = nh_.subscribe("command_twist", 1, &CobLookatController::twist_cb, this);
00157 chain_vel_pub = nh_.advertise<brics_actuator::JointVelocities>(chain_vel_pub_topic_, 1);
00158 lookat_vel_pub = nh_.advertise<brics_actuator::JointVelocities>("lookat_command_vel", 1);
00159
00160
00162 last_q_ = KDL::JntArray(chain_.getNrOfJoints());
00163 last_q_dot_ = KDL::JntArray(chain_.getNrOfJoints());
00164
00165 ROS_INFO("...initialized!");
00166 }
00167
00168 void CobLookatController::run()
00169 {
00170 ROS_INFO("cob_lookat_controller...spinning");
00171 ros::spin();
00172 }
00173
00174
00175 void CobLookatController::twist_cb(const geometry_msgs::Twist::ConstPtr& msg)
00176 {
00177 tf::StampedTransform transform_tf;
00178 KDL::Frame frame;
00179 try{
00180 tf_listener_.lookupTransform(chain_base_, chain_tip_, ros::Time(0), transform_tf);
00181 frame.p = KDL::Vector(transform_tf.getOrigin().x(), transform_tf.getOrigin().y(), transform_tf.getOrigin().z());
00182 frame.M = KDL::Rotation::Quaternion(transform_tf.getRotation().x(), transform_tf.getRotation().y(), transform_tf.getRotation().z(), transform_tf.getRotation().w());
00183 }
00184 catch (tf::TransformException ex){
00185 ROS_ERROR("%s",ex.what());
00186 return;
00187 }
00188
00189 KDL::Twist twist;
00190 tf::twistMsgToKDL(*msg, twist);
00191 KDL::JntArray q_dot_ik(chain_.getNrOfJoints());
00192
00193
00194
00195
00196
00198 KDL::Twist twist_transformed = frame*twist;
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212 int ret_ik = p_iksolver_vel_wdls_->CartToJnt(last_q_, twist_transformed, q_dot_ik);
00213
00214 if(ret_ik < 0)
00215 {
00216 ROS_ERROR("No Vel-IK found!");
00217 }
00218 else
00219 {
00220
00221
00222
00223
00224
00225
00226
00227 brics_actuator::JointVelocities chain_vel_msg;
00228 chain_vel_msg.velocities.resize(chain_dof_);
00229 for(int i=0; i<chain_dof_; i++)
00230 {
00231 chain_vel_msg.velocities[i].joint_uri = chain_joints_[i].c_str();
00232 chain_vel_msg.velocities[i].unit = "rad";
00233 if(std::fabs(q_dot_ik(i)) >= chain_limits_vel_[i])
00234 {
00235 ROS_WARN("JointVel %s: %f exceeds limit %f", chain_joints_[i].c_str(), q_dot_ik(i), chain_limits_vel_[i]);
00236
00237 chain_vel_msg.velocities[i].value = chain_limits_vel_[i];
00238 }
00239 else
00240 {
00241 chain_vel_msg.velocities[i].value = q_dot_ik(i);
00242 }
00243
00244 }
00245 brics_actuator::JointVelocities lookat_vel_msg;
00246 lookat_vel_msg.velocities.resize(lookat_dof_);
00247 for(int i=0; i<lookat_dof_; i++)
00248 {
00249 lookat_vel_msg.velocities[i].joint_uri = lookat_joints_[i].c_str();
00250 lookat_vel_msg.velocities[i].unit = "rad";
00251 lookat_vel_msg.velocities[i].value = q_dot_ik(chain_dof_ + i);
00252 }
00253
00254 chain_vel_pub.publish(chain_vel_msg);
00255 lookat_vel_pub.publish(lookat_vel_msg);
00256 }
00257 }
00258
00259
00260 void CobLookatController::jointstate_cb(const sensor_msgs::JointState::ConstPtr& msg)
00261 {
00262 KDL::JntArray q_temp = last_q_;
00263 KDL::JntArray q_dot_temp = last_q_dot_;
00264 int chain_count = 0;
00265 int lookat_count = 0;
00266
00267 for(unsigned int j = 0; j < chain_dof_; j++)
00268 {
00269 for(unsigned int i = 0; i < msg->name.size(); i++)
00270 {
00271 if(strcmp(msg->name[i].c_str(), chain_joints_[j].c_str()) == 0)
00272 {
00273 q_temp(j) = msg->position[i];
00274 q_dot_temp(j) = msg->velocity[i];
00275 chain_count++;
00276 break;
00277 }
00278 }
00279 }
00280 for(unsigned int j = 0; j < lookat_dof_; j++)
00281 {
00282 for(unsigned int i = 0; i < msg->name.size(); i++)
00283 {
00284 if(strcmp(msg->name[i].c_str(), lookat_joints_[j].c_str()) == 0)
00285 {
00286 q_temp(chain_dof_ + j) = msg->position[i];
00287 q_dot_temp(chain_dof_ + j) = msg->velocity[i];
00288 lookat_count++;
00289 break;
00290 }
00291 }
00292 }
00293
00294 if(chain_count == chain_dof_ || lookat_count == lookat_dof_)
00295 {
00296 ROS_DEBUG("Done Parsing");
00297 last_q_ = q_temp;
00298 last_q_dot_ = q_dot_temp;
00299 }
00300 }
00301
00302
00303
00304
00305
00306
00307
00308
00309
00310
00311
00312
00313
00314
00315
00316
00317
00318
00319
00320
00321
00322
00323
00324
00325
00326
00327
00328
00329
00330 void CobLookatController::safety_stop_tracking()
00331 {
00332 ros::ServiceClient savety_client = nh_.serviceClient<std_srvs::Empty>("/lookat_controller/stop_tracking");
00333 std_srvs::Empty srv_save_stop;
00334 srv_save_stop.request;
00335 if (savety_client.call(srv_save_stop)) {
00336 ROS_INFO("Lookat stopped for savety issues");
00337 }
00338 else {
00339 ROS_ERROR("Lookat stop failed! FATAL!");
00340 }
00341 }