Skip to content

Commit

Permalink
updated setpoint transform using tf2 instead of manual rotation matrix
Browse files Browse the repository at this point in the history
  • Loading branch information
@mzhouURI
mzhouURI committed Jun 24, 2024
1 parent c4c9413 commit 75f8108
Show file tree
Hide file tree
Showing 2 changed files with 101 additions and 52 deletions.
149 changes: 98 additions & 51 deletions src/mvp_control/mvp_control_ros.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1447,73 +1447,120 @@ bool MvpControlROS::f_amend_set_point(
}

Eigen::Vector3d p_world, rpy_world;
Eigen::Vector3d p_world2, rpy_world2;
try {
// Transform the position of setpoint frame_id to world_link
auto tf_world_setpoint = m_transform_buffer.lookupTransform(
m_world_link_id,
set_point.header.frame_id,
ros::Time(0)
);
if (abs(tf_world_setpoint.header.stamp.toSec() - ros::Time::now().toSec()) < 10 || tf_world_setpoint.header.stamp.toSec()==0.0)
{

auto tf_eigen = tf2::transformToEigen(tf_world_setpoint);

p_world = tf_eigen.rotation() *
Eigen::Vector3d(set_point.position.x, set_point.position.y, set_point.position.z)
+ tf_eigen.translation();
///convert euler angle into a different frame
///find the rotation matrix from the set point frame to the desired pose.
Eigen::Matrix3d R;
R = Eigen::AngleAxisd(set_point.orientation.z, Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(set_point.orientation.y, Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(set_point.orientation.x, Eigen::Vector3d::UnitX());

//find rotation matrix from the world link to the setpoint frame.
auto tf_1 = m_transform_buffer.lookupTransform(
set_point.header.frame_id,
m_world_link_id,
ros::Time(0)
);

if (abs(tf_1.header.stamp.toSec() - ros::Time::now().toSec()) < 10 || tf_1.header.stamp.toSec()==0.0)
{

auto tf_1_eigen = tf2::transformToEigen(tf_1);
//find the total rotation matrix from the world link to the desired pose.
Eigen::Matrix3d R_set_point = tf_1_eigen.rotation() *R;
// Eigen::Vector3d euler_angles = R_set_point.eulerAngles(2,1,0); // ZYX order
// rpy_world.z() = euler_angles[0];
// rpy_world.y() = euler_angles[1];
// rpy_world.x() = euler_angles[2];

// printf("from eigen: %lf, %lf, %lf\r\n",rpy_world.x(), rpy_world.y(), rpy_world.z());

//pitch angle
rpy_world.y() = asin(-R_set_point(2, 0));

// Calculate yaw (rotation about Z-axis)
rpy_world.z() = atan2(R_set_point(1, 0), R_set_point(0, 0));

// Calculate roll (rotation about X-axis)
rpy_world.x() = atan2(R_set_point(2, 1), R_set_point(2, 2));
// printf("from code: %lf, %lf, %lf\r\n",rpy_world.x(), rpy_world.y(), rpy_world.z());
//New approach
if (abs(tf_world_setpoint.header.stamp.toSec() - ros::Time::now().toSec()) < 10 || tf_world_setpoint.header.stamp.toSec()==0.0)
{
geometry_msgs::PoseStamped setpoint_origin, setpoint_converted;
setpoint_origin.header = set_point.header;
setpoint_origin.pose.position.x = set_point.position.x;
setpoint_origin.pose.position.y = set_point.position.y;
setpoint_origin.pose.position.z = set_point.position.z;
tf2::Quaternion q;
q.setRPY(set_point.orientation.x, set_point.orientation.y, set_point.orientation.z);
setpoint_origin.pose.orientation.x = q.x();
setpoint_origin.pose.orientation.y = q.y();
setpoint_origin.pose.orientation.z = q.z();
setpoint_origin.pose.orientation.w = q.w();

//convert
setpoint_converted.header = set_point.header;
setpoint_converted.header.frame_id = m_world_link_id;

tf2::doTransform(setpoint_origin, setpoint_converted,tf_world_setpoint);
tf2::Quaternion quat;
quat.setW(setpoint_converted.pose.orientation.w);
quat.setX(setpoint_converted.pose.orientation.x);
quat.setY(setpoint_converted.pose.orientation.y);
quat.setZ(setpoint_converted.pose.orientation.z);


p_world.x() = setpoint_converted.pose.position.x;
p_world.y() = setpoint_converted.pose.position.y;
p_world.z() = setpoint_converted.pose.position.z;

}
else
{
ROS_WARN( "%s to %s TF too old!", set_point.header.frame_id.c_str(), m_world_link_id.c_str() );
return false;
}
tf2::Matrix3x3(quat).getRPY(
rpy_world.x(),
rpy_world.y(),
rpy_world.z()
);
printf("setpoint frame=%s\r\n", set_point.header.frame_id.c_str());
// std::cout<<"xyz =\n"<<p_world<<std::endl;
std::cout<<"rpy =\n"<<rpy_world<<std::endl;
}
else
{
ROS_WARN( "%s to %s TF too old!", m_world_link_id.c_str(), set_point.header.frame_id.c_str() );
return false;
}
// rpy_world = tf_eigen.rotation() *
// Eigen::Vector3d(set_point.orientation.x, set_point.orientation.y, set_point.orientation.z);

// if (abs(tf_world_setpoint.header.stamp.toSec() - ros::Time::now().toSec()) < 10 || tf_world_setpoint.header.stamp.toSec()==0.0)
// {

// auto tf_eigen = tf2::transformToEigen(tf_world_setpoint);

// p_world2 = tf_eigen.rotation() *
// Eigen::Vector3d(set_point.position.x, set_point.position.y, set_point.position.z)
// + tf_eigen.translation();
// ///convert euler angle into a different frame
// ///find the rotation matrix from the set point frame to the desired pose.
// Eigen::Matrix3d R;
// R = Eigen::AngleAxisd(set_point.orientation.z, Eigen::Vector3d::UnitZ()) *
// Eigen::AngleAxisd(set_point.orientation.y, Eigen::Vector3d::UnitY()) *
// Eigen::AngleAxisd(set_point.orientation.x, Eigen::Vector3d::UnitX());

// //find rotation matrix from the world link to the setpoint frame.
// auto tf_1 = m_transform_buffer.lookupTransform(
// set_point.header.frame_id,
// m_world_link_id,
// ros::Time(0)
// );

// if (abs(tf_1.header.stamp.toSec() - ros::Time::now().toSec()) < 10 || tf_1.header.stamp.toSec()==0.0)
// {

// auto tf_1_eigen = tf2::transformToEigen(tf_1);
// //find the total rotation matrix from the world link to the desired pose.
// Eigen::Matrix3d R_set_point = tf_1_eigen.rotation() *R;
// // Eigen::Vector3d euler_angles = R_set_point.eulerAngles(2,1,0); // ZYX order
// // rpy_world.z() = euler_angles[0];
// // rpy_world.y() = euler_angles[1];
// // rpy_world.x() = euler_angles[2];

// // printf("from eigen: %lf, %lf, %lf\r\n",rpy_world.x(), rpy_world.y(), rpy_world.z());

// //pitch angle
// rpy_world2.y() = asin(-R_set_point(2, 0));

// // Calculate yaw (rotation about Z-axis)
// rpy_world2.z() = atan2(R_set_point(1, 0), R_set_point(0, 0));

// // Calculate roll (rotation about X-axis)
// rpy_world2.x() = atan2(R_set_point(2, 1), R_set_point(2, 2));

// // printf("from code: %lf, %lf, %lf\r\n",rpy_world.x(), rpy_world.y(), rpy_world.z());
// std::cout<<"original xyz =\n"<<p_world2<<std::endl;
// std::cout<<"original rpy =\n"<<rpy_world2<<std::endl;
// }
// else
// {
// ROS_WARN( "%s to %s TF too old!", set_point.header.frame_id.c_str(), m_world_link_id.c_str() );
// return false;
// }
// }
// else
// {
// ROS_WARN( "%s to %s TF too old!", m_world_link_id.c_str(), set_point.header.frame_id.c_str() );
// return false;
// }

//assume the set point uvw and pqr are in the m_cg_link_id

Expand Down
4 changes: 3 additions & 1 deletion src/mvp_control/mvp_control_ros.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,13 +37,15 @@
#include "tf2/LinearMath/Matrix3x3.h"
#include "tf2_eigen/tf2_eigen.h"
#include "tf2_ros/transform_listener.h"
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2/LinearMath/Quaternion.h>

#include "std_msgs/Float32.h"
#include "std_srvs/Empty.h"
#include "std_srvs/Trigger.h"
#include "nav_msgs/Odometry.h"
#include "dynamic_reconfigure/server.h"

#include "geometry_msgs/PoseStamped.h"
#include "mvp_control/PIDConfig.h"

#include "mvp_msgs/PIDgains.h"
Expand Down

0 comments on commit 75f8108

Please sign in to comment.