merge with rviz branch

.gitignore

@@ -15,3 +15,6 @@ CMakeLists.txt.user
 
 # Ignore PDFs on master
 literature/
+
+# Pictures stuff
+*.png

CMakeLists.txt

@@ -51,4 +51,7 @@ catkin_install_python(PROGRAMS
 )
 
 add_executable(aruco_detector src/aruco_detector.cpp)
+add_executable(rviz_human src/rviz_human.cpp)
+
 target_link_libraries(aruco_detector ${catkin_LIBRARIES})
+target_link_libraries(rviz_human ${catkin_LIBRARIES})

script/cartesian_controller.py

@@ -0,0 +1,139 @@
+#! /usr/bin/env python
+import os 
+
+import rospy
+import numpy as np
+
+import sys
+from naoqi import ALProxy
+import motion
+
+motionProxy = 0
+
+def get_transform(joint):
+    frame = motion.FRAME_TORSO
+    useSensorValues = True
+    result = motionProxy.getTransform(joint,frame,useSensorValues)
+    result = np.matrix(result)
+    print result
+    result = np.reshape(result, (4,4))
+    print result
+    return result
+
+
+def cartesian_position(joint):
+    print 'function'
+    frame = motion.FRAME_TORSO
+    useSensorValues = True
+    result = motionProxy.getPosition(joint, frame, useSensorValues)
+    #print result
+    return np.array(result[:3])
+
+
+def jacobian():
+
+    # get current positions/ accordint to control figure these values should actually come from the
+    # integration step in the previous first control loop
+
+    end_position = cartesian_position('LArm')
+
+    shoulder_position = cartesian_position('LShoulderPitch')
+    bicep_position = cartesian_position('LShoulderRoll')
+    elbow_position = cartesian_position('LElbowYaw')
+    forearm_position = cartesian_position('LElbowRoll')
+
+    # get transformed rotation axes, transformation to torso frame
+
+    x_axis = np.array([[1, 0, 0, 1]]).T
+    y_axis = np.array([[0, 1, 0, 1]]).T
+    z_axis = np.array([[0, 0, 1, 1]]).T
+    
+    shoulder_axis = get_transform('LShoulderPitch').dot(y_axis)
+    bicep_axis = get_transform('LShoulderRoll').dot(z_axis)
+    elbow_axis = get_transform('LElbowYaw').dot(x_axis)
+    forearm_axis = get_transform('LElbowRoll').dot(z_axis)
+
+    # get basis vectors of jacobian
+    
+    shoulder_basis = np.cross(shoulder_axis[:3].flatten(), end_position - shoulder_position)
+    bicep_basis = np.cross(bicep_axis[:3].flatten(), end_position - bicep_position)
+    elbow_basis = np.cross(elbow_axis[:3].flatten(), end_position - elbow_position)
+    forearm_basis = np.cross(forearm_axis[:3].flatten(), end_position - forearm_position)
+
+    # build jacobian matrix
+    jacobian = np.concatenate([shoulder_basis, bicep_basis, elbow_basis, forearm_basis], axis=0).T
+
+    return jacobian
+
+def pseudo_inverse(jacobian):
+    return np.linalg.pinv(jacobian)
+
+
+def reference_generator(p_d)
+
+    # calculate jacobian
+    jac_mat = jacobian()   
+
+    # use jacobian to compute desired joint speed
+    
+    derivative_speed = (p_d - end_position) / 5
+    inv_jac = pseudo_inverse(jac_mat)
+    angular_velocity = derivative_speed * inv_jac
+    
+        
+    # integrate over desired speed to get desired joint position
+    
+    names         = "LArm"
+    useSensors    = False
+    commandAnglesLArm = motionProxy.getAngles(names, useSensors)
+
+    #names         = "RArm"
+    #useSensors    = False
+    #commandAngles = motionProxy.getAngles(names, useSensors)
+
+    goal_angleL = commandAnglesLArm + (angular_velocity * 5)
+    
+
+    # return desired joint position and speed
+    
+    
+
+    return goal_angleL, commandAnglesLArm
+
+
+
+def movement(e)
+
+    # scale joint states with matrix K
+
+    # add desired joint speed
+
+    # move robot arm    
+
+    return
+ 
+
+if __name__ == '__main__':
+
+    motionProxy = ALProxy("ALMotion", os.environ['NAO_IP'], 9559)
+    jacob = jacobian()
+    print jacob
+    jacob = pseudo_inverse(jacob)
+    print(jacob)
+
+    # given new desired coordinates
+
+    e = 1;
+    """ 
+    while e bigger some value
+
+        # run reference generator to get desired joint postion and speed
+
+        # subtract current joint states
+
+        # movement
+ 
+    """    
+    #rospy.init_node('cartesian_controller')
+    #rospy.spin()
+

src/NAO_Jacobian.cpp

@@ -0,0 +1,61 @@
+#include <Vector3.h>
+
+using namespace std;
+// Calculate the Jacobian Matrix for the NAO
+// inputs: original angles tau1 - tau4 and new angles tau1' - tau4'
+// original endeffector position e1 - e4 and new endeffector position e1' - e4'
+ 
+typedef struct position{
+    float x;
+    float y;
+    float z;
+}position;
+
+typedef struct angles{
+    float tau_1;
+    float tau_2;
+    float tau_3;
+    float tau_4;
+}angles;
+
+angles a_end, a, a_diff;
+position e_end, e, e_diff;
+
+e_diff = diff(e_end, e);
+a_diff = diff(a_end, a);
+
+vector<float> postion_vec;
+postion_vec.vector::push_back(e_diff.x);
+postion_vec.vector::push_back(e_diff.y);
+postion_vec.vector::push_back(e_diff.z);
+vector<float> angles_vec;
+angles_vec.vector::push_back(a_diff.tau_1);
+angles_vec.vector::push_back(a_diff.tau_2);
+angles_vec.vector::push_back(a_diff.tau_3);
+angles_vec.vector::push_back(a_diff.tau_4);s
+vector<vector<float>> Jacobian;
+
+
+for (int i = 0; i<3; i++) {
+    for(int j = 0; j<4; j++ ) {
+        Jacobian[i][j] = postion_vec[i]/angles_vec[j];
+    }
+}
+
+position diff(position end, position actual){
+    position temp;
+    temp.x = end.x - actual.x;
+    temp.y = end.y - actual.y;
+    temp.z = end.z - actual.z;
+    return temp;
+}
+
+angles diff(angles end, angles actual){
+    angles temp;
+    temp.tau_1 = end.tau_1 - actual.tau_1;
+    temp.tau_2 = end.tau_2 - actual.tau_2;
+    temp.tau_3 = end.tau_3 - actual.tau_3;
+    temp.tau_4 = end.tau_4 - actual.tau_4;
+
+    return temp;
+}

src/rviz_human.cpp

@@ -0,0 +1,404 @@
+#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);
+
+
+int main( int argc, char** argv )
+{
+  ros::init(argc, argv, "rviz_human");
+  ros::NodeHandle n;
+  ros::Rate r(100);
+  ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>(
+          "visualization_marker", 10
+          );
+
+  tf::TransformListener tll;
+
+  int i = 0;
+  int walk = 0;
+  float l_leg_y = 0.0;
+  float r_leg_y = 0.0;
+
+  // suppose we already have the position of the center marker
+  // in the correct frame
+
+  float x_start = 2;
+  float y_start = 0;
+  float z_start = 1.3;
+
+  float x_0 = 2;
+  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
+    tf::StampedTransform aruco_0_tf;
+    tf::StampedTransform aruco_1_tf;
+    tf::StampedTransform aruco_2_tf;
+
+    try {
+        tll.lookupTransform(
+                "/odom", "/Aruco_0_frame",ros::Time(0), aruco_0_tf);
+        /*ROS_INFO("0: %f",aruco_0_tf.getOrigin()[0]);
+        ROS_INFO("1: %f",aruco_0_tf.getOrigin()[1]);
+        ROS_INFO("2: %f",aruco_0_tf.getOrigin()[2]);
+        */
+
+        tll.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();
+    }
+
+    try{
+    */
+
+    catch (tf::TransformException ex) {
+        ROS_ERROR("%s",ex.what());
+        ros::Duration(1.0).sleep();
+    }
+
+
+    visualization_msgs::Marker body;
+    visualization_msgs::Marker head;
+    visualization_msgs::Marker l_leg;
+    visualization_msgs::Marker r_leg;
+    visualization_msgs::Marker l_arm;
+    visualization_msgs::Marker r_arm;
+
+    visualization_msgs::Marker camera;
+
+    // Set the frame ID and timestamp.
+    // See the TF tutorials for information on these.
+    body.header.frame_id = "/odom";
+    body.header.stamp = ros::Time::now();
+
+    head.header.frame_id = "/odom";
+    head.header.stamp = ros::Time::now();
+
+    l_leg.header.frame_id = "/odom";
+    l_leg.header.stamp = ros::Time::now();
+
+    r_leg.header.frame_id = "/odom";
+    r_leg.header.stamp = ros::Time::now();
+
+    l_arm.header.frame_id = "/odom";
+    l_arm.header.stamp = ros::Time::now();
+
+    r_arm.header.frame_id = "/odom";
+    r_arm.header.stamp = ros::Time::now();
+
+    camera.header.frame_id = "/odom";
+    camera.header.stamp = ros::Time::now();
+
+    body.ns = "body";
+    body.id = 0;
+
+    head.ns = "head";
+    head.id = 1;
+
+    l_leg.ns = "l_leg";
+    l_leg.id = 2;
+
+    r_leg.ns = "r_leg";
+    r_leg.id = 3;
+
+    l_arm.ns = "l_arm";
+    l_arm.id = 4;
+
+    r_arm.ns = "r_arm";
+    r_arm.id = 5;
+
+    camera.ns = "r_arm";
+    camera.id = 6;
+
+    body.type = visualization_msgs::Marker::CUBE;
+    head.type = visualization_msgs::Marker::SPHERE;
+    l_leg.type = visualization_msgs::Marker::CYLINDER;
+    r_leg.type = visualization_msgs::Marker::CYLINDER;
+    l_arm.type = visualization_msgs::Marker::CYLINDER;
+    r_arm.type = visualization_msgs::Marker::CYLINDER;
+    camera.type = visualization_msgs::Marker::CUBE;
+
+    // Set the marker action.
+    // Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
+
+    body.action = visualization_msgs::Marker::ADD;
+    head.action = visualization_msgs::Marker::ADD;
+    l_arm.action = visualization_msgs::Marker::ADD;
+    r_arm.action = visualization_msgs::Marker::ADD;
+    l_leg.action = visualization_msgs::Marker::ADD;
+    r_arm.action = visualization_msgs::Marker::ADD;
+    camera.action = visualization_msgs::Marker::ADD;
+
+
+    // Set the pose of the marker.
+    // This is a full 6DOF pose relative to the
+    // frame/time specified in the header
+
+    body.pose.position.x = x_0;
+    body.pose.position.y = y_0;
+    body.pose.position.z = z_0;
+    body.pose.orientation.x = 0.0;
+    body.pose.orientation.y = 0.0;
+    body.pose.orientation.z = 0.0;
+    body.pose.orientation.w = 1.0;
+
+    head.pose.position.x = x_0;
+    head.pose.position.y = y_0;
+    head.pose.position.z = z_0+0.5; //1.85
+    head.pose.orientation.x = 0.0;
+    head.pose.orientation.y = 0.0;
+    head.pose.orientation.z = 0.0;
+    head.pose.orientation.w = 1.0;
+
+    l_leg.pose.position.x = x_0;
+    l_leg.pose.position.y = y_0+0.2;
+    l_leg.pose.position.z = z_0-0.8; //0.4
+    l_leg.pose.orientation.x = 0.0;
+    l_leg.pose.orientation.y = l_leg_y;
+    l_leg.pose.orientation.z = 0.0;
+    l_leg.pose.orientation.w = 1.0;
+
+    r_leg.pose.position.x = x_0;
+    r_leg.pose.position.y = y_0-0.2;
+    r_leg.pose.position.z = z_0-0.8; //0.4
+    r_leg.pose.orientation.x = 0.0;
+    r_leg.pose.orientation.y = r_leg_y;
+    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+delta_y;
+    l_arm.pose.position.z = z_0+0.3+delta_z; //1.6
+    l_arm.pose.orientation.x = 0.0+q.x;//+alpha;
+    l_arm.pose.orientation.y = 0.0+q.y;
+    l_arm.pose.orientation.z = 0.0+q.z;
+    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;
+    r_arm.pose.position.z = z_0+0.3; //1.6
+    r_arm.pose.orientation.x = 1.0;
+    r_arm.pose.orientation.y = 0.0;
+    r_arm.pose.orientation.z = 0.0;
+    r_arm.pose.orientation.w = 1.0;
+
+    camera.pose.position.x = 0;
+    camera.pose.position.y = 0;
+    camera.pose.position.z = 0.5;
+    camera.pose.orientation.x = 0.0;
+    camera.pose.orientation.y = 0.0;
+    camera.pose.orientation.z = 0.0;
+    camera.pose.orientation.w = 0.0;
+
+
+    // Set the scale of the marker -- 1x1x1 here means 1m on a side
+
+    body.scale.x = 0.2;
+    body.scale.y = 0.4;
+    body.scale.z = 0.8;
+
+    head.scale.x = 0.3;
+    head.scale.y = 0.3;
+    head.scale.z = 0.4;
+
+    l_leg.scale.x = 0.2;
+    l_leg.scale.y = 0.2;
+    l_leg.scale.z = 0.9;
+
+    r_leg.scale.x = 0.2;
+    r_leg.scale.y = 0.2;
+    r_leg.scale.z = 0.9;
+
+    l_arm.scale.x = 0.2;
+    l_arm.scale.y = 0.2;
+    l_arm.scale.z = 0.5;
+
+    r_arm.scale.x = 0.2;
+    r_arm.scale.y = 0.2;
+    r_arm.scale.z = 0.5;
+
+    camera.scale.x = 0.5;
+    camera.scale.y = 0.5;
+    camera.scale.z = 1;
+
+    // Set the color -- be sure to set alpha to something non-zero!
+
+    body.color.r = 0.0f;
+    body.color.g = 1.0f;
+    body.color.b = 0.5f;
+    body.color.a = 1.0;
+
+    head.color.r = 0.0f;
+    head.color.g = 1.0f;
+    head.color.b = 0.0f;
+    head.color.a = 1.0;
+
+    l_leg.color.r = 1.0f;
+    l_leg.color.g = 0.5f;
+    l_leg.color.b = 0.0f;
+    l_leg.color.a = 1.0;
+
+    r_leg.color.r = 1.0f;
+    r_leg.color.g = 0.5f;
+    r_leg.color.b = 0.0f;
+    r_leg.color.a = 1.0;
+
+    l_arm.color.r = 1.0f;
+    l_arm.color.g = 0.5f;
+    l_arm.color.b = 1.0f;
+    l_arm.color.a = 1.0;
+
+    r_arm.color.r = 1.0f;
+    r_arm.color.g = 0.5f;
+    r_arm.color.b = 1.0f;
+    r_arm.color.a = 1.0;
+
+    camera.color.r = 1.0f;
+    camera.color.g = 1.0f;
+    camera.color.b = 1.0f;
+    camera.color.a = 1.0;
+
+
+    body.lifetime = ros::Duration();
+    head.lifetime = ros::Duration();
+    l_leg.lifetime = ros::Duration();
+    r_leg.lifetime = ros::Duration();
+    l_arm.lifetime = ros::Duration();
+    r_arm.lifetime = ros::Duration();
+    camera.lifetime = ros::Duration();
+
+
+    // Publish the marker
+
+    while (marker_pub.getNumSubscribers() < 1)
+    {
+      if (!ros::ok())
+      {
+        return 0;
+      }
+      ROS_WARN_ONCE("Please create a subscriber to the marker");
+      sleep(1);
+    }
+
+
+
+    // little walking animation
+    i++;
+    if(i % 100 == 0)
+    {
+        if(walk == 0)
+        {
+            walk = 1;
+            l_leg_y = 0.2;
+            r_leg_y = -0.2;
+        }
+        else
+        {
+            l_leg_y = -0.2;
+            r_leg_y = 0.2;
+            walk = 0;
+        }
+    }
+
+
+    // move hole human
+    i++;
+
+    // publish markers
+
+    body.action = visualization_msgs::Marker::ADD;
+    head.action = visualization_msgs::Marker::ADD;
+    l_arm.action = visualization_msgs::Marker::ADD;
+    r_arm.action = visualization_msgs::Marker::ADD;
+    l_leg.action = visualization_msgs::Marker::ADD;
+    r_arm.action = visualization_msgs::Marker::ADD;
+
+    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);
+    marker_pub.publish(camera);
+    i = i +1;
+
+    r.sleep();
+
+  }
+
+}
+
+quarternion toQuaternion( double yaw, double pitch, double roll) {
+    // 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;
+}
+
+