diff --git a/script/cartesian_controller.py b/script/cartesian_controller.py index ea626a5..60f49c1 100755 --- a/script/cartesian_controller.py +++ b/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" - useSensors = False - commandAngles = motionProxy.getAngles(names, useSensors) + #names = "RArm" + #useSensors = False + #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) diff --git a/src/aruco_detector.cpp b/src/aruco_detector.cpp index 391c6e5..2b87349 100644 --- a/src/aruco_detector.cpp +++ b/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 diff --git a/src/rviz_human.cpp b/src/rviz_human.cpp index 550c870..96e2be8 100644 --- a/src/rviz_human.cpp +++ b/src/rviz_human.cpp @@ -28,10 +28,21 @@ */ #include +#include #include #include #include +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_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 - tf::StampedTransform transform; + // 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{ - listener.lookupTransform("/torso", "/aruco_0_frame",ros::Time(0), transform); - ROS_INFO("0: %f",transform.getOrigin()[0]); - ROS_INFO("1: %f",transform.getOrigin()[1]); - ROS_INFO("2: %f",transform.getOrigin()[2]); + aruco_0.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]); + */ + + 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; - } - 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; + try{ + */ - } - - x_0 = -i*0.001+x_start; + catch (tf::TransformException ex){ + ROS_ERROR("%s",ex.what()); + 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.z = z_0+0.3; //1.6 - l_arm.pose.orientation.x = 1.0; - l_arm.pose.orientation.y = 0.0; - l_arm.pose.orientation.z = 0.0; - l_arm.pose.orientation.w = 1.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; @@ -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; +} +