diff --git a/.gitignore b/.gitignore index c09a496..a0f2dbd 100644 --- a/.gitignore +++ b/.gitignore @@ -15,3 +15,6 @@ CMakeLists.txt.user # Ignore PDFs on master literature/ + +# Pictures stuff +*.png diff --git a/CMakeLists.txt b/CMakeLists.txt index 6f6d813..2ccd6ce 100644 --- a/CMakeLists.txt +++ b/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}) diff --git a/script/cartesian_controller.py b/script/cartesian_controller.py new file mode 100755 index 0000000..60f49c1 --- /dev/null +++ b/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() + diff --git a/src/NAO_Jacobian.cpp b/src/NAO_Jacobian.cpp new file mode 100644 index 0000000..92a1453 --- /dev/null +++ b/src/NAO_Jacobian.cpp @@ -0,0 +1,61 @@ +#include + +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 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 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> 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; +} diff --git a/src/rviz_human.cpp b/src/rviz_human.cpp new file mode 100644 index 0000000..2224163 --- /dev/null +++ b/src/rviz_human.cpp @@ -0,0 +1,404 @@ +#include +#include +#include +#include +#include + +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_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; +} + +