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; }