int ComauSmartSix::ik(float x, float y,float z,float qx, float qy,float qz, float qw, float* q_in,float* q_out) {
KDL::Rotation rot = KDL::Rotation::Quaternion(qx,qy,qz,qw);
double roll,pitch,yaw;
rot.GetRPY(roll,pitch,yaw);
return this->ik(x,y,z,roll,pitch,yaw,q_in,q_out,true);
}
void goal_callback(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
if(msg->header.frame_id.compare(frame_id_) != 0)
{
ROS_WARN("frame_id of goal did not match expected '%s'. Ignoring goal",
frame_id_.c_str());
return;
}
// copying over goal
state_[joint_names_.size() + 0] = msg->pose.position.x;
state_[joint_names_.size() + 1] = msg->pose.position.y;
state_[joint_names_.size() + 2] = msg->pose.position.z;
KDL::Rotation rot;
tf::quaternionMsgToKDL(msg->pose.orientation, rot);
rot.GetEulerZYX(state_[joint_names_.size() + 3], state_[joint_names_.size() + 4],
state_[joint_names_.size() + 5]);
if (!controller_started_)
{
if (controller_.start(state_, nWSR_))
{
ROS_INFO("Controller started.");
controller_started_ = true;
}
else
{
ROS_ERROR("Couldn't start controller.");
print_eigen(state_);
}
}
}
void Controller::calcPoseDiff(){
// Calculate the position difference expressed in the world frame
KDL::Vector diff_world(m_goal_pose.x - m_current_pose[0], m_goal_pose.y - m_current_pose[1], 0.0);
// Calculate the YouBot-to-world orientation matrix, based on the angle
// estimation.
KDL::Rotation rot = KDL::Rotation::RotZ(m_current_pose[2]);
// Express the position difference in the YouBot frame
m_delta_pose = rot.Inverse(diff_world);
m_delta_pose[2] = m_goal_pose.theta - m_current_pose[2];
}
Eigen::Quaterniond UpperBodyPlanner::Rmat2Quaternion(const Eigen::Matrix3d& inMat_) {
Eigen::Quaterniond outQ;
KDL::Rotation convert;
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
convert.data[3 * i + j] = inMat_(i,j);
}
}
convert.GetQuaternion(outQ.x(), outQ.y(), outQ.z(), outQ.w());
return outQ;
}
void DetectorFilter::composeTransform(const MatrixWrapper::ColumnVector& vector,
geometry_msgs::PoseWithCovarianceStamped& pose)
{
assert(vector.rows() == 6);
// construct kdl rotation from vector (x, y, z, Rx, Ry, Rz), and extract quaternion
KDL::Rotation rot = KDL::Rotation::RPY(vector(4), vector(5), vector(6));
rot.GetQuaternion(pose.pose.pose.orientation.x, pose.pose.pose.orientation.y,
pose.pose.pose.orientation.z, pose.pose.pose.orientation.w);
pose.pose.pose.position.x = vector(1);
pose.pose.pose.position.y = vector(2);
pose.pose.pose.position.z = vector(3);
};
controllerErrorCode ArmController::setHandCartesianPosLinear(double x, double y, double z, double roll, double pitch, double yaw, double timestep, double stepSize)
{
double pos;
//Set up controller
setMode(CONTROLLER_POSITION);
this->timeStep = timestep;
this->stepSize = stepSize;
//Calculate end position
KDL::Vector endPos(x,y,z);
KDL::Rotation endRot;
endRot = endRot.RPY(roll,pitch,yaw);
endFrame.p = endPos;
endFrame.M = endRot;
//Get current location
KDL::JntArray q = KDL::JntArray(pr2_kin.nJnts);
//Get joint positions
for(int i=0;i<ARM_MAX_JOINTS;i++){
armJointControllers[i].getPosAct(&pos);
q(i) = pos;
}
KDL::Frame f;
pr2_kin.FK(q,f);
startPosition = f.p; //Record Starting location
KDL::Vector move = endPos-startPosition; //Vector to ending position
double dist = move.Norm(); //Distance to move
moveDirection = move/dist; //Normalize movement vector
rotInterpolator.SetStartEnd(f.M, endRot);
double total_angle = rotInterpolator.Angle();
nSteps = (int)(dist/stepSize);
if(nSteps==0){
std::cout<<"ArmController.cpp: Error:: number of steps calculated to be 0"<<std::endl;
return CONTROLLER_COMPUTATION_ERROR;
}
angleStep = total_angle/nSteps;
lastTime= getTime(); //Record first time marker
stepIndex = 0; //Reset step index
linearInterpolation = true;
return CONTROLLER_ALL_OK;
}
void DetectorFilter::decomposeTransform(const geometry_msgs::PoseWithCovarianceStamped& pose,
MatrixWrapper::ColumnVector& vector)
{
assert(vector.rows() == 6);
// construct kdl rotation from quaternion, and extract RPY
KDL::Rotation rot = KDL::Rotation::Quaternion(pose.pose.pose.orientation.x,
pose.pose.pose.orientation.y,
pose.pose.pose.orientation.z,
pose.pose.pose.orientation.w);
rot.GetRPY(vector(4), vector(5), vector(6));
vector(1) = pose.pose.pose.position.x;
vector(2) = pose.pose.pose.position.y;
vector(3) = pose.pose.pose.position.z;
};
void YouBotBaseService::update()
{
if(is_in_visualization_mode)
{
in_odometry_state.read(m_odometry_state);
simxFloat pos[3];
pos[0] = m_odometry_state.pose.pose.position.x;
pos[1] = m_odometry_state.pose.pose.position.y;
pos[2] = m_odometry_state.pose.pose.position.z;
simxSetObjectPosition(clientID,all_robot_handle,-1,pos,simx_opmode_oneshot);
double euler[3];
simxFloat euler_s[3];
//different coordinate systems, fixing it here
KDL::Rotation orientation = KDL::Rotation::Quaternion(
m_odometry_state.pose.pose.orientation.x,
m_odometry_state.pose.pose.orientation.y,
m_odometry_state.pose.pose.orientation.z,
m_odometry_state.pose.pose.orientation.w);
// Instead of transforming for the inverse of this rotation,
// we should find the right transform. (this is legacy code that
// transforms vrep coords to rviz coords)
KDL::Rotation rotation = KDL::Rotation::RPY(0,-M_PI/2,M_PI).Inverse();
orientation = orientation * rotation;
orientation.GetRPY(euler[0],euler[1],euler[2]);
euler_s[0] = euler[0];
euler_s[1] = euler[1];
euler_s[2] = euler[2];
simxSetObjectOrientation(clientID,all_robot_handle,-1,euler_s,simx_opmode_oneshot);
simxSetJointPosition(clientID,vrep_joint_handle[0],m_joint_state.position[0], simx_opmode_oneshot);
simxSetJointPosition(clientID,vrep_joint_handle[1],m_joint_state.position[1], simx_opmode_oneshot);
simxSetJointPosition(clientID,vrep_joint_handle[2],m_joint_state.position[2], simx_opmode_oneshot);
simxSetJointPosition(clientID,vrep_joint_handle[3],m_joint_state.position[3], simx_opmode_oneshot);
return;
}
Hilas::IRobotBaseService::update();
}
void quaternionKDLToEigen(const KDL::Rotation &k, Eigen::Quaterniond &e)
{
// // is it faster to do this?
k.GetQuaternion(e.x(), e.y(), e.z(), e.w());
// or this?
//double x, y, z, w;
//k.GetQuaternion(x, y, z, w);
//e = Eigen::Quaterniond(w, x, y, z);
}
TEST(TFKDLConversions, tf_kdl_rotation)
{
tf::Quaternion t;
t[0] = gen_rand(-1.0,1.0);
t[1] = gen_rand(-1.0,1.0);
t[2] = gen_rand(-1.0,1.0);
t[3] = gen_rand(-1.0,1.0);
t.normalize();
KDL::Rotation k;
RotationTFToKDL(t,k);
double x,y,z,w;
k.GetQuaternion(x,y,z,w);
ASSERT_NEAR(fabs(t[0]),fabs(x),1e-6);
ASSERT_NEAR(fabs(t[1]),fabs(y),1e-6);
ASSERT_NEAR(fabs(t[2]),fabs(z),1e-6);
ASSERT_NEAR(fabs(t[3]),fabs(w),1e-6);
ASSERT_NEAR(t[0]*t[1]*t[2]*t[3],x*y*z*w,1e-6);
}
controllerErrorCode
ArmController::setHandCartesianPos(double x, double y, double z, double roll, double pitch, double yaw)
{
linearInterpolation = false;
//Define position and rotation
KDL::Vector position(x,y,z);
KDL::Rotation rotation;
rotation = rotation.RPY(roll,pitch,yaw);
cmdPos[0] = x;
cmdPos[1] = y;
cmdPos[2] = z;
cmdPos[3] = roll;
cmdPos[4] = pitch;
cmdPos[5] = yaw;
//Create frame based on position and rotation
KDL::Frame f;
f.p = position;
f.M = rotation;
return commandCartesianPos(f);
}
void yarp_IMU_interface::sense(KDL::Rotation &orientation,
KDL::Vector &linearAcceleration,
KDL::Vector &angularVelocity)
{
yarp::sig::Vector yOrientation;
yarp::sig::Vector yLinearAcceleration;
yarp::sig::Vector yAngularVelocity;
this->sense(yOrientation,
yLinearAcceleration,
yAngularVelocity);
orientation.Identity();
orientation = KDL::Rotation::RPY(yOrientation(0),
yOrientation(1),
yOrientation(2));
YarptoKDL(yLinearAcceleration, linearAcceleration);
YarptoKDL(yAngularVelocity, angularVelocity);
}
int main(int argc, char **argv)
{
float x=0.0f;
float y=0.0f;
float z=0.0f;
float roll=0.0f;
float pitch=0.0f;
float yaw=0.0f;
ros::init(argc, argv, "dan_tester");
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe( "lar_comau/comau_joint_states",1,state_callback );
ros::Publisher joint_state_pub = n.advertise<sensor_msgs::JointState>("lar_comau/comau_joint_state_publisher", 1);
ros::Publisher cartesian_controller = n.advertise<lar_comau::ComauCommand>("lar_comau/comau_cartesian_controller", 1);
ros::Rate loop_rate(10);
//robot
std::string robot_desc_string;
n.param("robot_description", robot_desc_string, std::string());
lar_comau::ComauSmartSix robot(robot_desc_string,"base_link", "link6");
//ROS_INFO("\n\n%s\n\n",argv[1]);
initPoses();
int count = 0;
while (ros::ok())
{
sensor_msgs::JointState msg;
geometry_msgs::Pose pose;
//std::stringstream ss;
//ss << "hello world " << count;
//msg.data = ss.str();
std::string command;
int command_index;
cout << "Please enter pose value: ";
cin >> command_index;
if(command_index<my_poses.size() && command_index>=0) {
std::cout << "OK command "<<command_index<<std::endl;
MyPose mypose = my_poses[command_index];
std::cout << mypose.x << "," <<mypose.y<<std::endl;
pose.position.x = mypose.x;
pose.position.y = mypose.y;
pose.position.z = mypose.z;
KDL::Rotation rot = KDL::Rotation::RPY(
mypose.roll*M_PI/180.0f,
mypose.pitch*M_PI/180.0f,
mypose.yaw*M_PI/180.0f
);
double qx,qy,qz,qw;
rot.GetQuaternion(qx,qy,qz,qw);
pose.orientation.x = qx;
pose.orientation.y = qy;
pose.orientation.z = qz;
pose.orientation.w = qw;
lar_comau::ComauCommand comau_command;
comau_command.pose = pose;
comau_command.command = "joint";
cartesian_controller.publish(comau_command);
ros::spinOnce();
std::cout << "Published "<<comau_command<<std::endl;
}
}
return 0;
}
geometry_msgs::Quaternion conversions::KDLRotToQuaternionMsg(const KDL::Rotation & in){
geometry_msgs::Quaternion out;
in.GetQuaternion(out.x, out.y, out.z, out.w);
return out;
}
bool CartesianGenerator::generateNewVelocityProfiles(RTT::base::PortInterface* portInterface){
m_time_passed = os::TimeService::Instance()->secondsSince(m_time_begin);
geometry_msgs::Pose pose;
cmdCartPose.read(pose);
#if 1
std::cout << "A new pose arrived" << std::endl;
std::cout << "position: " << pose.position.x << " " << pose.position.y << " " << pose.position.z << std::endl;
#endif
m_traject_end.p.x(pose.position.x);
m_traject_end.p.y(pose.position.y);
m_traject_end.p.z(pose.position.z);
m_traject_end.M=Rotation::Quaternion(pose.orientation.x,pose.orientation.y,pose.orientation.z,pose.orientation.w);
// get current position
geometry_msgs::Pose pose_meas;
m_position_meas_port.read(pose_meas);
m_traject_begin.p.x(pose_meas.position.x);
m_traject_begin.p.y(pose_meas.position.y);
m_traject_begin.p.z(pose_meas.position.z);
m_traject_begin.M=Rotation::Quaternion(pose_meas.orientation.x,pose_meas.orientation.y,pose_meas.orientation.z,pose_meas.orientation.w);
KDL::Rotation errorRotation = (m_traject_end.M)*(m_traject_begin.M.Inverse());
// KDL::Rotation tmp=errorRotation;
// double q1, q2, q3, q4;
// cout << "tmp" << endl;
// cout << tmp(0,0) << ", " << tmp(0,1) << ", " << tmp(0,2) << endl;
// cout << tmp(1,0) << ", " << tmp(1,1) << ", " << tmp(1,2) << endl;
// cout << tmp(2,0) << ", " << tmp(2,1) << ", " << tmp(2,2) << endl;
// cout << endl;
// tmp.GetQuaternion(q1,q2,q3,q4);
// cout << "tmp quaternion: " << q1 << q2 << q3 << q4 << endl;
double x,y,z,w;
errorRotation.GetQuaternion(x,y,z,w);
Eigen::AngleAxis<double> aa;
aa = Eigen::Quaterniond(w,x,y,z);
currentRotationalAxis = aa.axis();
deltaTheta = aa.angle();
// std::cout << "----EIGEN---------" << std::endl << "currentRotationalAxis: " << std::endl << currentRotationalAxis << std::endl;
// std::cout << "deltaTheta" << deltaTheta << std::endl;
// currentRotationalAxis[0]=x;
// currentRotationalAxis[1]=y;
// currentRotationalAxis[2]=z;
// if(currentRotationalAxis.norm() > 0.001){
// currentRotationalAxis.normalize();
// deltaTheta = 2*acos(w);
// }else{
// currentRotationalAxis.setZero();
// deltaTheta = 0.0;
// }
// std::cout << "----KDL-----------" << std::endl << "currentRotationalAxis: " << std::endl << currentRotationalAxis << std::endl;
// std::cout << "deltaTheta" << deltaTheta << std::endl;
std::vector<double> cartPositionCmd = std::vector<double>(3,0.0);
cartPositionCmd[0] = pose.position.x;
cartPositionCmd[1] = pose.position.y;
cartPositionCmd[2] = pose.position.z;
std::vector<double> cartPositionMsr = std::vector<double>(3,0.0);
//the following 3 assignments will get over written except for the first time
cartPositionMsr[0] = pose_meas.position.x;
cartPositionMsr[1] = pose_meas.position.y;
cartPositionMsr[2] = pose_meas.position.z;
std::vector<double> cartVelocity = std::vector<double>(3,0.0);
if ((int)motionProfile.size() == 0){//Only for the first run
for(int i = 0; i < 3; i++)
{
cartVelocity[i] = 0.0;
}
}else{
for(int i = 0; i < 3; i++)
{
cartVelocity[i] = motionProfile[i].Vel(m_time_passed);
cartPositionMsr[i] = motionProfile[i].Pos(m_time_passed);
}
}
motionProfile.clear();
// Set motion profiles
for(int i = 0; i < 3; i++){
motionProfile.push_back(VelocityProfile_NonZeroInit(m_maximum_velocity[i], m_maximum_acceleration[i]));
motionProfile[i].SetProfile(cartPositionMsr[i], cartPositionCmd[i], cartVelocity[i]);
}
//motionProfile for theta
motionProfile.push_back(VelocityProfile_NonZeroInit(m_maximum_velocity[3], m_maximum_acceleration[3]));
motionProfile[3].SetProfile(0.0,deltaTheta,0.0);
m_time_begin = os::TimeService::Instance()->getTicks();
m_time_passed = 0;
return true;
}
void quaternionKDLToTF(const KDL::Rotation &k, tf::Quaternion &t)
{
double x, y, z, w;
k.GetQuaternion(x, y, z, w);
t = tf::Quaternion(x, y, z, w);
}
void cloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg)
{
PointCloud cloud;
pcl::fromROSMsg(*msg, cloud);
g_cloud_frame = cloud.header.frame_id;
g_cloud_ready = true;
if(!g_head_depth_ready) return;
Mat img3D;
img3D = Mat::zeros(cloud.height, cloud.width, CV_32FC3);
//img3D.create(cloud.height, cloud.width, CV_32FC3);
int yMin, xMin, yMax, xMax;
yMin = 0; xMin = 0;
yMax = img3D.rows; xMax = img3D.cols;
//get 3D from depth
for(int y = yMin ; y < img3D.rows; y++) {
Vec3f* img3Di = img3D.ptr<Vec3f>(y);
for(int x = xMin; x < img3D.cols; x++) {
pcl::PointXYZ p = cloud.at(x,y);
if((p.z>g_head_depth-0.2) && (p.z<g_head_depth+0.2)) {
img3Di[x][0] = p.x*1000;
img3Di[x][1] = p.y*1000;
img3Di[x][2] = hypot(img3Di[x][0], p.z*1000); //they seem to have trained with incorrectly projected 3D points
//img3Di[x][2] = p.z*1000;
} else {
img3Di[x] = 0;
}
}
}
g_means.clear();
g_votes.clear();
g_clusters.clear();
//do the actual estimate
estimator.estimate( img3D,
g_means,
g_clusters,
g_votes,
g_stride,
g_maxv,
g_prob_th,
g_larger_radius_ratio,
g_smaller_radius_ratio,
false,
g_th
);
//assuming there's only one head in the image!
if(g_means.size() > 0) {
geometry_msgs::PoseStamped pose_msg;
pose_msg.header.frame_id = msg->header.frame_id;
cv::Vec<float,POSE_SIZE> pose(g_means[0]);
KDL::Rotation r = KDL::Rotation::RPY(
from_degrees( pose[5]+180+g_roll_bias ),
from_degrees(-pose[3]+180+g_pitch_bias),
from_degrees(-pose[4]+ g_yaw_bias )
);
double qx, qy, qz, qw;
r.GetQuaternion(qx, qy, qz, qw);
geometry_msgs::PointStamped head_point;
geometry_msgs::PointStamped head_point_transformed;
head_point.header = pose_msg.header;
head_point.point.x = pose[0]/1000;
head_point.point.y = pose[1]/1000;
head_point.point.z = pose[2]/1000;
try {
listener->waitForTransform(head_point.header.frame_id, g_head_target_frame, ros::Time::now(), ros::Duration(2.0));
listener->transformPoint(g_head_target_frame, head_point, head_point_transformed);
} catch(tf::TransformException ex) {
ROS_WARN(
"Transform exception, when transforming point from %s to %s\ndropping pose (don't worry, this is probably ok)",
head_point.header.frame_id.c_str(), g_head_target_frame.c_str());
ROS_WARN("Exception was %s", ex.what());
return;
}
tf::Transform trans;
pose_msg.pose.position = head_point_transformed.point;
pose_msg.header.frame_id = head_point_transformed.header.frame_id;
pose_msg.pose.orientation.x = qx;
pose_msg.pose.orientation.y = qy;
pose_msg.pose.orientation.z = qz;
pose_msg.pose.orientation.w = qw;
trans.setOrigin(tf::Vector3(
pose_msg.pose.position.x,
pose_msg.pose.position.y,
pose_msg.pose.position.z
));
trans.setRotation(tf::Quaternion(qx, qy, qz, qw));
g_transform = tf::StampedTransform(trans, pose_msg.header.stamp, pose_msg.header.frame_id, "head_origin");
// broadcaster->sendTransform(tf::StampedTransform(trans, pose_msg.header.stamp, pose_msg.header.frame_id, "head_origin"));
g_transform_ready = true;
pose_msg.header.stamp = ros::Time::now();
geometry_msgs::PoseStamped zero_pose;
zero_pose.header.frame_id = "head_origin";
zero_pose.header.stamp = ros::Time::now();
zero_pose.pose.orientation.w = 1;
//pose_pub.publish(pose_msg);
pose_pub.publish(zero_pose);
}
}
void UrdfModelMarker::getJointState(boost::shared_ptr<const Link> link)
{
string link_frame_name_ = tf_prefix_ + link->name;
boost::shared_ptr<Joint> parent_joint = link->parent_joint;
if(parent_joint != NULL){
KDL::Frame initialFrame;
KDL::Frame presentFrame;
KDL::Rotation rot;
KDL::Vector rotVec;
KDL::Vector jointVec;
double jointAngle;
double jointAngleAllRange;
switch(parent_joint->type){
case Joint::REVOLUTE:
case Joint::CONTINUOUS:
{
linkProperty *link_property = &linkMarkerMap[link_frame_name_];
tf::poseMsgToKDL (link_property->initial_pose, initialFrame);
tf::poseMsgToKDL (link_property->pose, presentFrame);
rot = initialFrame.M.Inverse() * presentFrame.M;
jointAngle = rot.GetRotAngle(rotVec);
jointVec = KDL::Vector(link_property->joint_axis.x,
link_property->joint_axis.y,
link_property->joint_axis.z);
if( KDL::dot(rotVec,jointVec) < 0){
jointAngle = - jointAngle;
}
if(link_property->joint_angle > M_PI/2 && jointAngle < -M_PI/2){
link_property->rotation_count += 1;
}else if(link_property->joint_angle < -M_PI/2 && jointAngle > M_PI/2){
link_property->rotation_count -= 1;
}
link_property->joint_angle = jointAngle;
jointAngleAllRange = jointAngle + link_property->rotation_count * M_PI * 2;
if(parent_joint->type == Joint::REVOLUTE && parent_joint->limits != NULL){
bool changeMarkerAngle = false;
if(jointAngleAllRange < parent_joint->limits->lower){
jointAngleAllRange = parent_joint->limits->lower + 0.001;
changeMarkerAngle = true;
}
if(jointAngleAllRange > parent_joint->limits->upper){
jointAngleAllRange = parent_joint->limits->upper - 0.001;
changeMarkerAngle = true;
}
if(changeMarkerAngle){
setJointAngle(link, jointAngleAllRange);
}
}
joint_state_.position.push_back(jointAngleAllRange);
joint_state_.name.push_back(parent_joint->name);
break;
}
case Joint::PRISMATIC:
{
KDL::Vector pos;
linkProperty *link_property = &linkMarkerMap[link_frame_name_];
tf::poseMsgToKDL (link_property->initial_pose, initialFrame);
tf::poseMsgToKDL (link_property->pose, presentFrame);
pos = presentFrame.p - initialFrame.p;
jointVec = KDL::Vector(link_property->joint_axis.x,
link_property->joint_axis.y,
link_property->joint_axis.z);
jointVec = jointVec / jointVec.Norm(); // normalize vector
jointAngle = KDL::dot(jointVec, pos);
link_property->joint_angle = jointAngle;
jointAngleAllRange = jointAngle;
if(parent_joint->type == Joint::PRISMATIC && parent_joint->limits != NULL){
bool changeMarkerAngle = false;
if(jointAngleAllRange < parent_joint->limits->lower){
jointAngleAllRange = parent_joint->limits->lower + 0.003;
changeMarkerAngle = true;
}
if(jointAngleAllRange > parent_joint->limits->upper){
jointAngleAllRange = parent_joint->limits->upper - 0.003;
changeMarkerAngle = true;
}
if(changeMarkerAngle){
setJointAngle(link, jointAngleAllRange);
}
}
joint_state_.position.push_back(jointAngleAllRange);
joint_state_.name.push_back(parent_joint->name);
break;
}
case Joint::FIXED:
break;
default:
break;
}
server_->applyChanges();
}
for (std::vector<boost::shared_ptr<Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++){
getJointState(*child);
}
return;
}
bool CartesianGenerator::generateNewVelocityProfiles(RTT::base::PortInterface* portInterface)
{
m_time_passed = os::TimeService::Instance()->secondsSince(m_time_begin);
geometry_msgs::Pose pose;
cmdCartPose.read(pose);
desired_pose = pose;
#if 1
std::cout << "A new pose arrived" << std::endl;
std::cout << "position: " << pose.position.x << " " << pose.position.y << " " << pose.position.z << std::endl;
#endif
m_traject_end.p.x(pose.position.x);
m_traject_end.p.y(pose.position.y);
m_traject_end.p.z(pose.position.z);
m_traject_end.M = Rotation::Quaternion(pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w);
// get current position
geometry_msgs::Pose pose_meas;
m_position_meas_port.read(pose_meas);
m_traject_begin.p.x(pose_meas.position.x);
m_traject_begin.p.y(pose_meas.position.y);
m_traject_begin.p.z(pose_meas.position.z);
m_traject_begin.M = Rotation::Quaternion(pose_meas.orientation.x, pose_meas.orientation.y, pose_meas.orientation.z,
pose_meas.orientation.w);
xi = pose_meas.position.x;
yi = pose_meas.position.y;
zi = pose_meas.position.z;
xf = pose.position.x;
yf = pose.position.y;
zf = pose.position.z;
TrajVectorMagnitude = sqrt((xf - xi) * (xf - xi) + (yf - yi) * (yf - yi) + (zf - zi) * (zf - zi));
TrajVectorDirection.x = (xf - xi) / TrajVectorMagnitude;
TrajVectorDirection.y = (yf - yi) / TrajVectorMagnitude;
TrajVectorDirection.z = (zf - zi) / TrajVectorMagnitude;
double tx, ty, tz;
tx = abs(xf - xi) / m_maximum_velocity[0];
ty = abs(yf - yi) / m_maximum_velocity[0];
tz = abs(zf - zi) / m_maximum_velocity[0];
t_sync = TrajVectorMagnitude / m_maximum_velocity[0];
KDL::Rotation errorRotation = (m_traject_end.M) * (m_traject_begin.M.Inverse());
double x, y, z, w;
errorRotation.GetQuaternion(x, y, z, w);
Eigen::AngleAxis<double> aa;
aa = Eigen::Quaterniond(w, x, y, z);
currentRotationalAxis = aa.axis();
deltaTheta = aa.angle();
theta_vel = deltaTheta / t_sync;
cout << "[CG::GenerateProfiles]:" << endl;
m_time_begin = os::TimeService::Instance()->getTicks();
m_time_passed = 0;
return true;
}
// construct rotation
urdf::Rotation toUrdf(const KDL::Rotation & r)
{
double x,y,z,w;
r.GetQuaternion(x,y,z,w);
return urdf::Rotation(x,y,z,w);
}
bool publishMarkerArray(geometry_msgs::Pose axis, std::vector<geometry_msgs::Pose> trajectoyPoints, std::string refFrameName){
visualization_msgs::MarkerArray tmpMarkerArray;
KDL::Rotation axisR = KDL::Rotation::Quaternion(axis.orientation.x, axis.orientation.y,axis.orientation.z,axis.orientation.w);
KDL::Rotation deltaR = KDL::Rotation::Quaternion(0.0,std::sin(-PI/4),0.0,std::cos(-PI/4));
KDL::Rotation markerR = axisR*deltaR;
visualization_msgs::Marker axisMarker;
axisMarker.header.frame_id = refFrameName;
axisMarker.header.stamp = ros::Time();
axisMarker.ns = "articulationNS";
axisMarker.id = 0;
axisMarker.type = visualization_msgs::Marker::ARROW;
axisMarker.action = visualization_msgs::Marker::ADD;
axisMarker.scale.x = 2;
axisMarker.scale.y = 2;
axisMarker.scale.z = 2;
axisMarker.color.a = 1.0;
axisMarker.color.r = 0.0;
axisMarker.color.g = 1.0;
axisMarker.color.b = 0.0;
//std::cout << "Before: "<<axisMarker.pose.orientation.x << " " << axisMarker.pose.orientation.y << " " << axisMarker.pose.orientation.z << " " << axisMarker.pose.orientation.w << std::endl;
markerR.GetQuaternion(axisMarker.pose.orientation.x,axisMarker.pose.orientation.y,axisMarker.pose.orientation.z,axisMarker.pose.orientation.w);
//std::cout << "After: "<<axisMarker.pose.orientation.x << " " << axisMarker.pose.orientation.y << " " << axisMarker.pose.orientation.z << " " << axisMarker.pose.orientation.w << std::endl;
visualization_msgs::Marker axisText;
axisText.header.frame_id = refFrameName;
axisText.header.stamp = ros::Time();
axisText.ns = "articulationNS";
axisText.id = 1;
axisText.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
axisText.action = visualization_msgs::Marker::ADD;
axisText.scale.x = 0.5;
axisText.scale.y = 0.5;
axisText.scale.z = 0.5;
axisText.color.a = 1.0;
axisText.color.r = 1.0;
axisText.color.g = 1.0;
axisText.color.b = 1.0;
visualization_msgs::Marker trajPoint;
trajPoint.header.frame_id = refFrameName;
trajPoint.header.stamp = ros::Time();
trajPoint.ns = "articulationNS";
trajPoint.type = visualization_msgs::Marker::SPHERE;
trajPoint.action = visualization_msgs::Marker::ADD;
trajPoint.scale.x = .3;
trajPoint.scale.y = .3;
trajPoint.scale.z = .3;
trajPoint.color.a = 1.0;
trajPoint.color.r = 0.0;
trajPoint.color.g = 1.0;
trajPoint.color.b = 0.0;
axisText.pose = axis;
//std::stringstream s;
//s << "(" << axisText.pose.position.x << ", " << axisText.pose.position.y << ")" ;
axisText.text = "Axis of Rotation";
tmpMarkerArray.markers.push_back(axisMarker);
tmpMarkerArray.markers.push_back(axisText);
for(int i=0; i<(int)trajectoyPoints.size(); i++){
trajPoint.pose = trajectoyPoints[i];
trajPoint.id = 2 + i;
tmpMarkerArray.markers.push_back(trajPoint);
}
ROS_INFO("Publishing MarkerArray: Size=%d",(int)tmpMarkerArray.markers.size());
vis_pub.publish( tmpMarkerArray );
return true;
}