pavel/teleoperation
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

tried to implement arm rotation with quarternions

Browse Source
This commit is contained in:
HRS_D
2019-02-05 18:30:24 +01:00
parent 3b16ae6fcd
commit 2d1b3ba1b6
3 changed files with 108 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
script/cartesian_controller.py
View File

@@ -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)
goal_angleL = commandAnglesLArm + (angular_velocity * 5)
names = "RArm"
useSensors = False
commandAngles = motionProxy.getAngles(names, useSensors)
goal_angle = mom_angle + (angular_velocity * 5)
# return desired joint position and speed
return
return goal_angleL, commandAnglesLArm
def movement(e)

2
src/aruco_detector.cpp
View File

@@ -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

152
src/rviz_human.cpp
View File

@@ -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
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{
*/
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
}
x_0 = -i*0.001+x_start;
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;
}