wpi_jaco_wrapper: jaco_conversions.cpp Source File

Go to the documentation of this file.
00001 #include <wpi_jaco_wrapper/jaco_conversions.h>
00002 
00003 using namespace std;
00004 
00005 JacoConversions::JacoConversions(void)
00006 {
00007   loadParameters(n);
00008 
00009   //advertise service
00010   eqServer = n.advertiseService(topic_prefix_ + "_conversions/euler_to_quaternion", &JacoConversions::callEQ, this);
00011   qeServer = n.advertiseService(topic_prefix_ + "_conversions/quaternion_to_euler", &JacoConversions::callQE, this);
00012 }
00013 
00014 bool JacoConversions::callEQ(wpi_jaco_msgs::EulerToQuaternion::Request &req,
00015                              wpi_jaco_msgs::EulerToQuaternion::Response &res)
00016 {
00017   float t1 = req.roll;
00018   float t2 = req.pitch;
00019   float t3 = req.yaw;
00020 
00021   // Calculate the quaternion given roll, pitch, and yaw (rotation order XYZ -- 1:X, 2:Y, 3:Z)
00022   res.orientation.w = -sin(t1 / 2.0) * sin(t2 / 2.0) * sin(t3 / 2.0) + cos(t1 / 2.0) * cos(t2 / 2.0) * cos(t3 / 2.0);
00023   res.orientation.x = sin(t1 / 2.0) * cos(t2 / 2.0) * cos(t3 / 2.0) + sin(t2 / 2.0) * sin(t3 / 2.0) * cos(t1 / 2.0);
00024   res.orientation.y = -sin(t1 / 2.0) * sin(t3 / 2.0) * cos(t2 / 2.0) + sin(t2 / 2.0) * cos(t1 / 2.0) * cos(t3 / 2.0);
00025   res.orientation.z = sin(t1 / 2.0) * sin(t2 / 2.0) * cos(t3 / 2.0) + sin(t3 / 2.0) * cos(t2 / 2.0) * cos(t2 / 2.0);
00026 
00027   return true;
00028 }
00029 
00030 bool JacoConversions::callQE(wpi_jaco_msgs::QuaternionToEuler::Request &req,
00031                              wpi_jaco_msgs::QuaternionToEuler::Response &res)
00032 {
00033   float q1 = req.orientation.w;
00034   float q2 = req.orientation.x;
00035   float q3 = req.orientation.y;
00036   float q4 = req.orientation.z;
00037 
00038   // Calculate necessary elements of the rotation matrix
00039   float m11 = pow(q1, 2) + pow(q2, 2) - pow(q3, 2) - pow(q4, 2);
00040   float m12 = 2 * (q2 * q3 - q1 * q4);
00041   float m13 = 2 * (q2 * q4 + q1 * q3);
00042   float m23 = 2 * (q3 * q4 - q1 * q2);
00043   float m33 = pow(q1, 2) - pow(q2, 2) - pow(q3, 2) + pow(q4, 2);
00044 
00045   // Calculate the roll, pitch, and yaw (rotation order XYZ -- 1:X, 2:Y, 3:Z)
00046   res.roll = atan2(-m23, m33);
00047   res.pitch = atan2(m13, sqrt(1 - pow(m13, 2)));
00048   res.yaw = atan2(-m12, m11);
00049 
00050   return true;
00051 }
00052 
00053 bool JacoConversions::loadParameters(const ros::NodeHandle n)
00054 {
00055     n.param("wpi_jaco/arm_name", arm_name_, std::string("jaco"));
00056 
00057     // Update topic prefix
00058     if (arm_name_ == "jaco2")
00059         topic_prefix_ = "jaco";
00060     else
00061         topic_prefix_ = arm_name_;
00062 
00064     return true;
00065 }
00066 
00067 int main(int argc, char **argv)
00068 {
00069   ros::init(argc, argv, "jaco_conversions");
00070 
00071   JacoConversions jc;
00072 
00073   ros::spin();
00074 }
00075