void GetActiveCameraPose(const geometry_msgs::PoseStamped& msg)
{
cameraPose.set_p(Vector3d(msg.pose.position.x, msg.pose.position.y, msg.pose.position.z));
cameraPose.set_o(Quaternion<double>(msg.pose.orientation.w, msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z));
first_camera_pose_read = true;
}
void omniPoseOriginCallback(const geometry_msgs::PoseStamped& msg)
{
omniPoseOrigin.set_p(Vector3d(msg.pose.position.x, msg.pose.position.y, msg.pose.position.z));
omniPoseOrigin.set_o(Quaternion<double>(msg.pose.orientation.w, msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z));
// std::cout << "traj gen pose: " << omniPose.p().transpose() << std::endl; // TEST TEST
}
// Subscribers
void omniPoseCallback(const geometry_msgs::PoseStamped& msg)
{
omniPose.set_p(Vector3d(msg.pose.position.x, msg.pose.position.y, msg.pose.position.z));
omniPose.set_o(Quaternion<double>(msg.pose.orientation.w, msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z));
omni_data_read = true;
std::cout << "omni pose: " << omniPose.p().transpose() << std::endl; // TEST TEST
}
robotPose trajectory_generator::trajectory_horizontal_circle_no_rotation(const double time){
double radius = 2;
double angular_vel = (2*PI)/5;
pose_.set_p(radius*cos(angular_vel*time), radius*sin(angular_vel*time), 1);
pose_.set_o(1.0, 0.0, 0.0, 0.0);
return pose_;
}
KDL::Frame pose_to_frame(robotPose pose){
return KDL::Frame(
KDL::Rotation( pose.o_rotmat()(0,0), pose.o_rotmat()(1,0), pose.o_rotmat()(2,0),
pose.o_rotmat()(0,1), pose.o_rotmat()(1,1), pose.o_rotmat()(2,1),
pose.o_rotmat()(0,2), pose.o_rotmat()(1,2), pose.o_rotmat()(2,2)),
KDL::Vector( pose.px(), pose.py(), pose.pz()));
}
void trajectory_generator::init_data(void){
iteration_ = 0;
pose_.set_p(0.0, 0.0, 0.0);
pose_.set_o(1.0, 0.0, 0.0, 0.0);
origin_pose_.set_p(0.0, 0.0, 0.0);
origin_pose_.set_o(1.0, 0.0, 0.0, 0.0);
max_pos_velocity_ = 1; // m/s
max_angle_velocity_ = DEG_TO_RAD(1);
total_time_ = 0;
gettimeofday(¤t_time_, NULL);
}
void trajectory_generator::set_trajectory_origin(const Vector3d position, const Quaternion<double> orientation){
origin_pose_.set_p(position);
origin_pose_.set_o(orientation);
}
int main(int argc, char* argv[])
{
srand(time(NULL));
//*****************************************************************************************************************************************
// ROS
ros::init(argc, argv, "omni_teleop_bubble_cam");
ros::NodeHandle n;
ros::Rate loop_rate(loop_rate_in_hz);
ROS_INFO("omni_teleop_bubble_cam launched...");
// Kautham
ros::Publisher pubClient_Move = n.advertise<trajectory_msgs::JointTrajectoryPoint>("Move_Robots", 1);
ros::ServiceClient srvClient_Setup = n.serviceClient<kautham_ros::ProblemSetup>("Problem_Setup");
ros::ServiceClient srvClient_Collision = n.serviceClient<kautham_ros::ReqCollisionCheck>("Request_Collision_Check");
ros::ServiceClient srvKViewOn = n.serviceClient<kautham_ros::kViewOn>("Request_KauthamViewer_ON");
// Camera
ros::Publisher pubSetActiveCamera = n.advertise<geometry_msgs::PoseStamped>("Set_ActiveCamera_Pose", 1);
ros::Subscriber subGetActiveCamera = n.subscribe("Get_ActiveCamera_Pose", 1, &GetActiveCameraPose);
// Omni
ros::Publisher pubWStfScale = n.advertise<geometry_msgs::Vector3>("scale", 1);
ros::ServiceClient srvWsTfSetAlg = n.serviceClient<workspace_transformation::setAlgorithm>("set_algorithm");
ros::Subscriber subOmniPose = n.subscribe("poseStampedSlaveWorkspace", 1, &omniPoseCallback);
ros::Subscriber subOmniPoseOrigin = n.subscribe("poseStampedSlaveWorkspaceOrigin", 1, &omniPoseOriginCallback);
ros::Subscriber subOmniButtons = n.subscribe("button", 1, &omniButtonCallback);
ros::Subscriber sub_HapticAngles = n.subscribe("angles", 1, &HapticAnglesCallback);
//*****************************************************************************************************************************************
// KAUTHAM
// Wait until kautham viewer is ready
bool kautham_viewer_ready = false;
kautham_ros::kViewOn kview_on_srv;
while (!kautham_viewer_ready){
srvKViewOn.call(kview_on_srv);
kautham_viewer_ready = kview_on_srv.response.kview_on;
}
// Get problem path string
string problem_path;
if (argc == 1){
problem_path = "/home/users/josep.a.claret/catkin_ws/src/kview_prenodes/nodes/bubble_teleop_cam_real/scenes/BMM_Teleop_spotcyl.xml";
}
else if (argc == 2){
problem_path = argv[1];
}
else{
cout << "Wrong number of arguments" << endl;
return 1;
}
// Load visualization parameters
Scene = new Robot(problem_path);
ROS_INFO("Loading Scene ...");
kautham_ros::ProblemSetup msgSetup;
msgSetup.request.problem.data = Scene->problemPath;
msgSetup.request.hand = 0;
srvClient_Setup.call(msgSetup);
if (msgSetup.response.status){
Scene->dim = 22;
}
else{
cout << "Problem with setup for Scene" << endl;
return 1;
}
kautham_ros::ReqCollisionCheck msgCollision;
//****************************************************************************************************************************************
// Setup world to kautham world transform parameters
// milimetres!!!!!!!!!!!!!!!
double Xmin, Xmax, Ymin, Ymax, Zmin, Zmax, Xrange, Yrange, Zrange;
Xmin = -5.0;
Ymin = -5.0;
Zmin = 0.0;
Xmax = 5.0;
Ymax = 5.0;
Zmax = 2.0;
Xrange = Xmax - Xmin;
Yrange = Ymax - Ymin;
Zrange = Zmax - Zmin;
//****************************************************************************************************************************************
// Control algorithm
unsigned int algorithm_type = 4;
//****************************************************************************************************************************************
// workspace transformation
omniPose.set_p(Vector3d(0.0, 0.0, 1.0));
omniPose.set_o(Quaternion<double>(1.0, 0.0, 0.0, 0.0));
geometry_msgs::Vector3 ws_tf_scale_vec;
ws_tf_scale_vec.x = 1.2;
ws_tf_scale_vec.y = 0.5;
ws_tf_scale_vec.z = 0.3;
pubWStfScale.publish(ws_tf_scale_vec);
ros::spinOnce();
//****************************************************************************************************************************************
// Camera
geometry_msgs::PoseStamped ros_camera_pose;
ros_camera_pose.pose.position.x = ros_camera_pose.pose.position.y = ros_camera_pose.pose.position.z = 0.0;
ros_camera_pose.pose.orientation.x = ros_camera_pose.pose.orientation.y = ros_camera_pose.pose.orientation.z = 0.0;
ros_camera_pose.pose.orientation.w = 1.0;
Eigen::Vector3d vCamera(2.5, 0.0, 0.0);
Eigen::Vector3d eigen_cam_axis_z = vCamera.normalized();
Eigen::Vector3d eigen_cam_axis_x = ( Eigen::Vector3d::UnitZ().cross( eigen_cam_axis_z ) ).normalized();
Eigen::Vector3d eigen_cam_axis_y = ( eigen_cam_axis_z.cross( eigen_cam_axis_x ) ).normalized();
Eigen::Transform<double,3,Affine> TF_WSOriginInScene_Camera;
TF_WSOriginInScene_Camera.translation() = vCamera;
TF_WSOriginInScene_Camera.linear() << eigen_cam_axis_x(0), eigen_cam_axis_y(0), eigen_cam_axis_z(0),
eigen_cam_axis_x(1), eigen_cam_axis_y(1), eigen_cam_axis_z(1),
eigen_cam_axis_x(2), eigen_cam_axis_y(2), eigen_cam_axis_z(2);
//****************************************************************************************************************************************
// Setting initial configuration
MobMa bmm;
controlEnv IKJacobian(bmm, 0);
IKJacobian.set_algorithm(algorithm_type);
Eigen::VectorXd current_config(bmm.dim()), next_config(bmm.dim());
current_config = Matrix<double,10,1>::Zero();
current_config(0) = current_config(1) = 0.0;
IKJacobian.set_current_config(current_config);
robotPose desired_pose;
desired_pose.set_p(-0.15, 0.0, 1.528);
desired_pose.set_o(1.0, 0.0, 0.0, 0.0);
desired_pose.plot("desired_pose");
IKJacobian.set_desired_pose(desired_pose);
IKJacobian.plot_data();
cout << "Entering the loop" << endl;
unsigned int n_it = 0;
while (ros::ok())
{
pubWStfScale.publish(ws_tf_scale_vec);
if (change_algorithm){
if (algorithm_type == 4) algorithm_type++;
else algorithm_type--;
IKJacobian.set_algorithm(algorithm_type);
change_algorithm = false;
}
double platform_heigth = PLATFORM_HEIGTH;
omniPoseOrigin.set_p(Vector3d(omniPoseOrigin.px(), omniPoseOrigin.py(), platform_heigth+0.5));
// SECURITY CONSTRAINTS ------------------------------------------------------------------------------------------------------------------------
double z_low_limit = 0.9;
if (omniPose.pz() < z_low_limit) omniPose.set_p(omniPose.px(), omniPose.py(), z_low_limit);
// End SECURITY CONSTRAINTS --------------------------------------------------------------------------------------------------------------------
omni_teleop_mm_data omni_teleop_mm_kview_data;
omni_teleop_mm_kview_data = omni_teleop_mm(&IKJacobian, &bmm, omniPose);
Vector3d platformPos(omni_teleop_mm_kview_data.platform_position);
Quaternion<double> PlatformRot_quat(omni_teleop_mm_kview_data.platform_quaternion);
double LWRangles[N_LWR_JOINTS];
for (unsigned int i=0; i<N_LWR_JOINTS; i++) LWRangles[i] = omni_teleop_mm_kview_data.manip_config(i);
Vector3d teleopPos(omni_teleop_mm_kview_data.teleop_position);
Quaternion<double> teleopRot_quat(omni_teleop_mm_kview_data.teleop_quaternion);
Vector3d teleopOriginPos(omniPoseOrigin.px(), omniPoseOrigin.py(), omniPoseOrigin.pz());
// Camera --------------------------------------------------------------------------------------------------------------------------------------
Eigen::Transform<double,3,Affine> TF_W_OmniWSOriginInScene;
TF_W_OmniWSOriginInScene.translation() << omniPoseOrigin.px(),omniPoseOrigin.py(),omniPoseOrigin.pz();
TF_W_OmniWSOriginInScene.linear() = omniPoseOrigin.o().toRotationMatrix();
Eigen::Transform<double,3,Affine> TF_W_Camera = TF_W_OmniWSOriginInScene * TF_WSOriginInScene_Camera;
// Publish to ROS
geometry_msgs::PoseStamped ros_camera_pose;
ros_camera_pose.pose.position.x = TF_W_Camera.translation()[0];
ros_camera_pose.pose.position.y = TF_W_Camera.translation()[1];
ros_camera_pose.pose.position.z = TF_W_Camera.translation()[2];
ros_camera_pose.pose.orientation.x = Quaternion<double>(TF_W_Camera.linear()).x();
ros_camera_pose.pose.orientation.y = Quaternion<double>(TF_W_Camera.linear()).y();
ros_camera_pose.pose.orientation.z = Quaternion<double>(TF_W_Camera.linear()).z();
ros_camera_pose.pose.orientation.w = Quaternion<double>(TF_W_Camera.linear()).w();
pubSetActiveCamera.publish(ros_camera_pose);
// Orientation transformations -----------------------------------------------------------------------------------------------------------------
// Platform - Quaternion to axis-angles: get the angles
double PlatformRot_AxisAngle_angle = Eigen::AngleAxis<double>(PlatformRot_quat).angle();
// Omni - Quaternion to x1, x2, x3
double x1, x2, x3, q_err;
q_err = 0.00001; // quaternion component tolerance
kautham_rotation_input kautham_rot;
kautham_rot = quat_to_kautham_rot(teleopRot_quat, q_err);
// cout << "0 - x1 x2 x3" << endl;
// cout << "1 - x1 x2 x3 " << kautham_rot.x1*(180/M_PI) << " " << kautham_rot.x2*(180/M_PI) << " " << kautham_rot.x3*(180/M_PI) << " " << endl;
Vector3d teleopRot(kautham_rot.x1, kautham_rot.x2 , kautham_rot.x3);
// From workspace to kautham workspace ---------------------------------------------------------------------------------------------------------
// Mobile Manipulator
// Platform
// Position
Vector3d KplatformPos((platformPos(0,0) - Xmin)/Xrange, (platformPos(1,0) - Ymin)/Yrange, 0.0);
// Orientation
double KplatformAng = PlatformRot_AxisAngle_angle/(2*M_PI);
// LWR
double KLWRangles[N_LWR_JOINTS];
KLWRangles[0] = ( LWRangles[0] - (-170)*(M_PI/180) )/( (170 - (-170))*(M_PI/180) );
KLWRangles[1] = ( LWRangles[1] - ( -30)*(M_PI/180) )/( (210 - ( -30))*(M_PI/180) );
KLWRangles[2] = ( LWRangles[2] - (-170)*(M_PI/180) )/( (170 - (-170))*(M_PI/180) );
KLWRangles[3] = ( LWRangles[3] - (-120)*(M_PI/180) )/( (120 - (-120))*(M_PI/180) );
KLWRangles[4] = ( LWRangles[4] - (-170)*(M_PI/180) )/( (170 - (-170))*(M_PI/180) );
KLWRangles[5] = ( LWRangles[5] - (-120)*(M_PI/180) )/( (120 - (-120))*(M_PI/180) );
KLWRangles[6] = ( LWRangles[6] - (-170)*(M_PI/180) )/( (170 - (-170))*(M_PI/180) );
// Omni
// Position
Vector3d KteleopPos((teleopPos.x() - Xmin)/Xrange, (teleopPos.y() - Ymin)/Yrange, (teleopPos.z() - Zmin)/Zrange);
// Orientation
Vector3d KteleopRot(teleopRot);
// Omni Origin - Position
Vector3d KteleopOriginPos((teleopOriginPos.x() - Xmin)/Xrange, (teleopOriginPos.y() - Ymin)/Yrange, (2.0 - Zmin)/Zrange);
// Construct kautham viewer data -------------------------------------------------------------------------------------------------------------
trajectory_msgs::JointTrajectoryPoint sceneConfiguration;
sceneConfiguration.positions.resize(N_KAUT_CONFIG);
sceneConfiguration.positions[0] = KplatformPos.x();
sceneConfiguration.positions[1] = KplatformPos.y();
sceneConfiguration.positions[2] = KplatformAng;
sceneConfiguration.positions[3] = KLWRangles[0]; // A1
sceneConfiguration.positions[4] = KLWRangles[1]; // A2
sceneConfiguration.positions[5] = KLWRangles[2]; // E1
sceneConfiguration.positions[6] = KLWRangles[3]; // A3
sceneConfiguration.positions[7] = KLWRangles[4]; // A4
sceneConfiguration.positions[8] = KLWRangles[5]; // A5
sceneConfiguration.positions[9] = KLWRangles[6]; // A6 - No l'he comprovat. assumeixo que està bé
sceneConfiguration.positions[10] = KteleopPos.x();
sceneConfiguration.positions[11] = KteleopPos.y();
sceneConfiguration.positions[12] = KteleopPos.z();
sceneConfiguration.positions[13] = KteleopRot.x();
sceneConfiguration.positions[14] = KteleopRot.y();
sceneConfiguration.positions[15] = KteleopRot.z();
// Haptic cilynder workspace
sceneConfiguration.positions[16] = KteleopPos.x();
sceneConfiguration.positions[17] = KteleopPos.y();
sceneConfiguration.positions[18] = 0.5;
// Cylinder shadow
sceneConfiguration.positions[19] = KteleopOriginPos.x();
sceneConfiguration.positions[20] = KteleopOriginPos.y();
sceneConfiguration.positions[21] = KteleopOriginPos.z();
//satura fins a una certa tolerància
double err_tol = 0.000001;
for (unsigned int i=0; i<N_KAUT_CONFIG; i++){
if ( (sceneConfiguration.positions[i] < 0) && (sceneConfiguration.positions[i] > -err_tol) ) sceneConfiguration.positions[i] = 0;
else if ( (sceneConfiguration.positions[i] > 1) && (sceneConfiguration.positions[i] < 1 + err_tol) ) sceneConfiguration.positions[i] = 1;
}
// Check if positions are inside workspace
for (unsigned int i=0; i<N_KAUT_CONFIG; i++){
if ( (sceneConfiguration.positions[i] < 0) || (sceneConfiguration.positions[i] > 1) ){
cout << "Element " << i << " in sceneConfiguration is " << sceneConfiguration.positions[i] << " -> out of bounds!!" << endl;
return 1;
}
}
// std::cout << " SC: "; for (unsigned int i=0; i<N_KAUT_CONFIG; i++) std::cout << sceneConfiguration.positions[i] << " "; std::cout << std::endl;
pubClient_Move.publish(sceneConfiguration);
ros::spinOnce();
loop_rate.sleep();
n_it++;
}
return 0;
}
int main(int argc, char* argv[])
{
srand(time(NULL));
ros::init(argc, argv, "omni_teleop_mm");
ros::NodeHandle n;
list<pair<unsigned, unsigned> >::iterator it_moves;
double loop_rate_in_hz = 20;
ros::Rate loop_rate(loop_rate_in_hz); // 10Hz
ROS_INFO("omni_teleop_platform_xyphi_by_velocity launched...");
// Phantom omni data
ros::Subscriber subOmniPose = n.subscribe("omni/poseStampedSlaveWorkspace", 1000, omniPoseCallback);
ros::Publisher pubWStfScale = n.advertise<geometry_msgs::Vector3>("omni/scale", 1);
ros::ServiceClient srvWsTfSetAlg = n.serviceClient<workspace_transformation::setAlgorithm>("omni/set_algorithm");
// BMM Platform
ros::Publisher bmm_platform_velocities_pub = n.advertise<sensor_msgs::JointState>("bmm_platform_velocity", 10);
// Variables
double v_x = 0.0;
double v_y = 0.0;
double v_phi = 0.0;
sensor_msgs::JointState platform_velocity;
platform_velocity.velocity.resize(3);
robotPose platform_pose;
bool workspace_transform_ready = false;
unsigned int ws_tf_algorithm_id = 0;;
workspace_transformation::setAlgorithm ws_tf_set_algorithm_data;
geometry_msgs::Vector3 ws_tf_scale_vec;
omni_data_read = false;
// workspace transformation
ws_tf_algorithm_id = 0; // Scaling control
ws_tf_set_algorithm_data.request.algorithm_number = ws_tf_algorithm_id;
while (!workspace_transform_ready){
srvWsTfSetAlg.call(ws_tf_set_algorithm_data);
workspace_transform_ready = ws_tf_set_algorithm_data.response.algorithm_number_ok;
}
ws_tf_scale_vec.x = 1.2;
ws_tf_scale_vec.y = 0.5;
ws_tf_scale_vec.z = 0.3;
pubWStfScale.publish(ws_tf_scale_vec);
ros::spinOnce();
cout << "Entering the loop" << endl;
unsigned int n_it = 0;
while (ros::ok())
{
pubWStfScale.publish(ws_tf_scale_vec);
if (omni_data_read){
// Workspace transformation
if (fabs(omniPose.px()) > 0.2) v_x = (omniPose.px()/fabs(omniPose.px()))*0.1;
else v_x = 0.0;
if (fabs(omniPose.py()) > 0.2) v_y = (omniPose.py()/fabs(omniPose.py()))*0.1;
else v_y = 0.0;
if (fabs(omniPose.pz()-1) > 0.1) v_phi = ((omniPose.pz()-1)/fabs(omniPose.pz()-1))*(PI/16);
else v_phi = 0.0;
platform_velocity.velocity[0] = -v_x;
platform_velocity.velocity[1] = -v_y;
platform_velocity.velocity[2] = v_phi;
bmm_platform_velocities_pub.publish(platform_velocity);
}
ros::spinOnce();
loop_rate.sleep();
n_it++;
}
return 0;
}
