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