tried to implement arm rotation with quarternions

script/cartesian_controller.py

@@ -65,7 +65,6 @@ def jacobian():
 
     return jacobian
 
-
 def pseudo_inverse(jacobian):
     return np.linalg.pinv(jacobian)
 
@@ -86,18 +85,21 @@ def reference_generator(p_d)
     
     names         = "LArm"
     useSensors    = False
-    commandAngles = motionProxy.getAngles(names, useSensors)
+    commandAnglesLArm = motionProxy.getAngles(names, useSensors)
 
-    names         = "RArm"
+    #names         = "RArm"
-    useSensors    = False
+    #useSensors    = False
-    commandAngles = motionProxy.getAngles(names, useSensors)
+    #commandAngles = motionProxy.getAngles(names, useSensors)
+
+    goal_angleL = commandAnglesLArm + (angular_velocity * 5)
+    
 
-    goal_angle = mom_angle + (angular_velocity * 5)
     # return desired joint position and speed
     
     
 
-    return
+    return goal_angleL, commandAnglesLArm
+
 
 
 def movement(e)

src/aruco_detector.cpp

@@ -29,7 +29,7 @@ class ImageConverter {
         loop_rate.sleep();
 
         // Subscribe to input video feed
-        image_sub_ = it_.subscribe("/usb_cam/image_raw", 1,
+        image_sub_ = it_.subscribe("/nao_robot/camera/front/image_raw", 1,
                 &ImageConverter::imageCb, this);
 
         // Create output windows

src/rviz_human.cpp

@@ -28,10 +28,21 @@
  */
 
 #include <iostream>
+#include <math.h> 
 #include <ros/ros.h>
 #include <visualization_msgs/Marker.h>
 #include <tf/transform_listener.h>
 
+struct quarternion {
+    double x;
+    double y;
+    double z;
+    double w;
+};
+
+
+quarternion toQuaternion( double yaw, double pitch, double roll); // yaw (Z), pitch (Y), roll (X)
+
 
 int main( int argc, char** argv )
 {
@@ -40,7 +51,9 @@ int main( int argc, char** argv )
   ros::Rate r(100);
   ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 10);
 
-  tf::TransformListener listener;
+  tf::TransformListener aruco_0;
+  tf::TransformListener aruco_1;
+  tf::TransformListener aruco_2;
 
   
   //uint32_t shape = visualization_msgs::Marker::CUBE;
@@ -61,45 +74,57 @@ int main( int argc, char** argv )
   float y_0 = 0;
   float z_0 = 1.3;
   
+  float x_1 = 0;
+  float y_1 = 0;
+  float z_1 = 0;
+
   while (ros::ok())
   {
 
-    /* // tried to subscribe to tf to recieve marker coordinates    
+     // tried to subscribe to tf to recieve marker coordinates    
-    tf::StampedTransform transform;
+    tf::StampedTransform aruco_0_tf;
+    tf::StampedTransform aruco_1_tf;
+    tf::StampedTransform aruco_2_tf;
 
     try{
-        listener.lookupTransform("/torso", "/aruco_0_frame",ros::Time(0), transform);
+        aruco_0.lookupTransform("/odom", "/Aruco_0_frame",ros::Time(0), aruco_0_tf);
-        ROS_INFO("0: %f",transform.getOrigin()[0]);
+        /*ROS_INFO("0: %f",aruco_0_tf.getOrigin()[0]);
-        ROS_INFO("1: %f",transform.getOrigin()[1]);
+        ROS_INFO("1: %f",aruco_0_tf.getOrigin()[1]);
-        ROS_INFO("2: %f",transform.getOrigin()[2]);
+        ROS_INFO("2: %f",aruco_0_tf.getOrigin()[2]);
+        */
+
+        aruco_1.lookupTransform("/Aruco_0_frame", "/Aruco_1_frame",ros::Time(0), aruco_1_tf);
+        /*
+        ROS_INFO("0_1: %f",aruco_1_tf.getOrigin()[0]);
+        ROS_INFO("1_1: %f",aruco_1_tf.getOrigin()[1]);
+        ROS_INFO("2_1: %f",aruco_1_tf.getOrigin()[2]);
+        */
+
+        x_0 = aruco_0_tf.getOrigin()[0];
+        y_0 = aruco_0_tf.getOrigin()[1];
+        z_0 = aruco_0_tf.getOrigin()[2];
+        
+        x_1 = aruco_1_tf.getOrigin()[0];
+        y_1 = aruco_1_tf.getOrigin()[1];
+        z_1 = aruco_1_tf.getOrigin()[2];
+
+
     }
+
+    /*
     catch (tf::TransformException ex){
         ROS_ERROR("%s",ex.what());
         ros::Duration(1.0).sleep();
     }
-   */
-    if(i == 300)
-    {
-        x_0 = 5;
 
-    } 
+    try{
-    if(i == 600)
+    */  
-    {
-        y_0 = 3;
-    } 
-    if(i == 900)
-    {
-        z_0 = 0.3;
-    } 
-    if(i == 1200)
-    {
-        x_0 = 2;
-        y_0 = 0;
-        z_0 = 1.3;
 
-    } 
+    catch (tf::TransformException ex){
-
+        ROS_ERROR("%s",ex.what());
-    x_0 = -i*0.001+x_start;
+        ros::Duration(1.0).sleep();
+    }
+   
 
     visualization_msgs::Marker body;
     visualization_msgs::Marker head;
@@ -206,13 +231,30 @@ int main( int argc, char** argv )
     r_leg.pose.orientation.z = 0.0;
     r_leg.pose.orientation.w = 1.0;
 
+    // calculate left arm angel in z y plane; arm length is 0.5
+
+    float alpha = atan(z_1/y_1);
+    float delta_z = 0;//0.25*sin(alpha);
+    float delta_y = 0;//0.35*cos(alpha);
+
+    //rad to degree
+    alpha = alpha*180/3.1415;
+    quarternion q = toQuaternion(0,0, alpha);
+
+    ROS_INFO("x: %f, y: %f, z: %f", x_1, y_1, z_1);
+    
+    ROS_INFO("alpha: %f, z: %f, y: %f", alpha, delta_z, delta_y); 
+
+    ROS_INFO("qx: %f, qy: %f, qz: %f, qw: %f", q.x,q.y,q.z,q.w); 
+    
+
     l_arm.pose.position.x = x_0;
-    l_arm.pose.position.y = y_0+0.4;
+    l_arm.pose.position.y = y_0+0.4+delta_y;
-    l_arm.pose.position.z = z_0+0.3; //1.6
+    l_arm.pose.position.z = z_0+0.3+delta_z; //1.6
-    l_arm.pose.orientation.x = 1.0;
+    l_arm.pose.orientation.x = 0.0+q.x;//+alpha;
-    l_arm.pose.orientation.y = 0.0;
+    l_arm.pose.orientation.y = 0.0+q.y;
-    l_arm.pose.orientation.z = 0.0;
+    l_arm.pose.orientation.z = 0.0+q.z;
-    l_arm.pose.orientation.w = 1.0;
+    l_arm.pose.orientation.w = 1.0+q.w;
 
     r_arm.pose.position.x = x_0;
     r_arm.pose.position.y = y_0-0.4;
@@ -321,7 +363,7 @@ int main( int argc, char** argv )
     }
 
     
-    /*
+    
     // little walking animation
     i++;
     if(i % 100 == 0)
@@ -339,7 +381,7 @@ int main( int argc, char** argv )
             walk = 0;
         }
     } 
-    */
+    
 
     // move hole human
     i++;
@@ -362,32 +404,30 @@ int main( int argc, char** argv )
     marker_pub.publish(camera);
     i = i +1;
     
-
-    //ROS_INFO("%d",i);
-
-      
-
     r.sleep();
   
-    // Delete markers
-    
-    body.action = visualization_msgs::Marker::DELETE;
-    head.action = visualization_msgs::Marker::DELETE;
-    l_arm.action = visualization_msgs::Marker::DELETE;
-    r_arm.action = visualization_msgs::Marker::DELETE;
-    l_leg.action = visualization_msgs::Marker::DELETE;
-    r_arm.action = visualization_msgs::Marker::DELETE;
-
-    marker_pub.publish(body);
-    marker_pub.publish(head);
-    marker_pub.publish(l_leg);   
-    marker_pub.publish(r_leg);
-    marker_pub.publish(l_arm);
-    marker_pub.publish(r_arm);  
-
-
   }
 
 }
 
+quarternion toQuaternion( double yaw, double pitch, double roll) // yaw (Z), pitch (Y), roll (X)
+{
+    // Abbreviations for the various angular functions
+    double cy = cos(yaw * 0.5);
+    double sy = sin(yaw * 0.5);
+    double cp = cos(pitch * 0.5);
+    double sp = sin(pitch * 0.5);
+    double cr = cos(roll * 0.5);
+    double sr = sin(roll * 0.5);
+
+    quarternion q;
+    
+    q.x = cy * cp * sr - sy * sp * cr;
+    q.y = sy * cp * sr + cy * sp * cr;
+    q.z = sy * cp * cr - cy * sp * sr;
+    q.w = cy * cp * cr + sy * sp * sr;
+   
+    return q;
+}
+