merge with rviz branch
This commit is contained in:
Pavel Lutskov
61
src/NAO_Jacobian.cpp
Normal file
61
src/NAO_Jacobian.cpp
Normal file
@@ -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;
|
||||
}
|
||||
404
src/rviz_human.cpp
Normal file
404
src/rviz_human.cpp
Normal file
@@ -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;
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user