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