int do_inits() { Eigen::Matrix3d R; Eigen::Vector3d nvec, tvec, bvec, tip_pos; bvec << 0, 0, 1; //default for testing: gripper points "down" nvec << 1, 0, 0; tvec = bvec.cross(nvec); R.col(0) = nvec; R.col(1) = tvec; R.col(2) = bvec; g_des_gripper_affine1.linear() = R; tip_pos << -0.15, -0.03, 0.07; g_des_gripper_affine1.translation() = tip_pos; //will change this, but start w/ something legal //hard-coded camera-to-base transform, useful for simple testing/debugging g_affine_lcamera_to_psm_one.translation() << -0.155, -0.03265, 0.0; nvec << -1, 0, 0; tvec << 0, 1, 0; bvec << 0, 0, -1; //Eigen::Matrix3d R; R.col(0) = nvec; R.col(1) = tvec; R.col(2) = bvec; g_affine_lcamera_to_psm_one.linear() = R; g_q_vec1_start.resize(7); g_q_vec1_goal.resize(7); g_q_vec2_start.resize(7); g_q_vec2_goal.resize(7); g_des_gripper1_wrt_base = g_affine_lcamera_to_psm_one.inverse() * g_des_gripper_affine1; g_ik_solver.ik_solve(g_des_gripper1_wrt_base); // compute IK: g_q_vec1_goal = g_ik_solver.get_soln(); g_q_vec1_start = g_q_vec1_goal; g_q_vec2_start << 0, 0, 0, 0, 0, 0, 0; //just put gripper 2 at home position g_q_vec2_goal << 0, 0, 0, 0, 0, 0, 0; //repackage q's into a trajectory; //populate a goal message for action server; // initialize with 2 poses that are identical g_trajectory_point1.positions.clear(); g_trajectory_point2.positions.clear(); //resize these: for (int i=0;i<14;i++) { g_trajectory_point1.positions.push_back(0.0); g_trajectory_point2.positions.push_back(0.0); } for (int i = 0; i < 7; i++) { g_trajectory_point1.positions[i] = g_q_vec1_start[i]; g_trajectory_point1.positions[i + 7] = g_q_vec2_start[i]; //should fix up jaw-opening values...do this later } g_trajectory_point1.time_from_start = ros::Duration(0.0); g_trajectory_point2.time_from_start = ros::Duration(2.0); g_des_trajectory.points.clear(); // can clear components, but not entire trajectory_msgs g_des_trajectory.joint_names.clear(); // des_trajectory.joint_names.push_back("right_s0"); //yada-yada should fill in names g_des_trajectory.header.stamp = ros::Time::now(); g_des_trajectory.points.push_back(g_trajectory_point1); // first point of the trajectory g_des_trajectory.points.push_back(g_trajectory_point2); // copy traj to goal: g_goal.trajectory = g_des_trajectory; g_got_new_pose = true; //send robot to start pose ROS_INFO("getting transforms from camera to PSMs"); tf::TransformListener tfListener; tf::StampedTransform tfResult_one, tfResult_two; bool tferr = true; int ntries = 0; ROS_INFO("waiting for tf between base and right_hand..."); while (tferr) { if (ntries > 5) break; //give up and accept default after this many tries tferr = false; try { //The direction of the transform returned will be from the target_frame to the source_frame. //Which if applied to data, will transform data in the source_frame into the target_frame. See tf/CoordinateFrameConventions#Transform_Direction tfListener.lookupTransform("left_camera_optical_frame", "one_psm_base_link", ros::Time(0), tfResult_one); //tfListener.lookupTransform("left_camera_optical_frame", "two_psm_base_link", ros::Time(0), tfResult_two); } catch (tf::TransformException &exception) { ROS_WARN("%s", exception.what()); tferr = true; ros::Duration(0.5).sleep(); // sleep for half a second ros::spinOnce(); ntries++; } } //default transform: need to match this up to camera calibration! if (tferr) { g_affine_lcamera_to_psm_one.translation() << -0.155, -0.03265, 0.0; Eigen::Vector3d nvec, tvec, bvec; nvec << -1, 0, 0; tvec << 0, 1, 0; bvec << 0, 0, -1; Eigen::Matrix3d R; R.col(0) = nvec; R.col(1) = tvec; R.col(2) = bvec; g_affine_lcamera_to_psm_one.linear() = R; g_affine_lcamera_to_psm_one.linear() = R; g_affine_lcamera_to_psm_one.translation() << 0.145, -0.03265, 0.0; ROS_WARN("using default transform"); } else { ROS_INFO("tf is good"); //affine_lcamera_to_psm_one is the position/orientation of psm1 base frame w/rt left camera link frame // need to extend this to camera optical frame g_affine_lcamera_to_psm_one = transformTFToEigen(tfResult_one); //affine_lcamera_to_psm_two = transformTFToEigen(tfResult_two); } ROS_INFO("transform from left camera to psm one:"); cout << g_affine_lcamera_to_psm_one.linear() << endl; cout << g_affine_lcamera_to_psm_one.translation().transpose() << endl; //ROS_INFO("transform from left camera to psm two:"); //cout << affine_lcamera_to_psm_two.linear() << endl; //cout << affine_lcamera_to_psm_two.translation().transpose() << endl; ROS_INFO("done w/ inits"); return 0; }