bool update_trajectory(double traj_clock, trajectory_msgs::JointTrajectory trajectory, Vectorq7x1 qvec_prev, int &isegment, Vectorq7x1 &qvec_new) {
trajectory_msgs::JointTrajectoryPoint trajectory_point_from, trajectory_point_to;
int njnts = qvec_prev.size();
Vectorq7x1 qvec, qvec_to, delta_qvec, dqvec;
int nsegs = trajectory.points.size() - 1;
double t_subgoal;
//cout<<"traj_clock = "<<traj_clock<<endl;
if (isegment < nsegs) {
trajectory_point_to = trajectory.points[isegment + 1];
t_subgoal = trajectory_point_to.time_from_start.toSec();
//cout<<"iseg = "<<isegment<<"; t_subgoal = "<<t_subgoal<<endl;
} else {
cout << "reached end of last segment" << endl;
trajectory_point_to = trajectory.points[nsegs];
t_subgoal = trajectory_point_to.time_from_start.toSec();
for (int i = 0; i < 7; i++) {
qvec_new[i] = trajectory_point_to.positions[i];
}
cout << "final time: " << t_subgoal << endl;
return false;
}
//cout<<"iseg = "<<isegment<<"; t_subgoal = "<<t_subgoal<<endl;
while ((t_subgoal < traj_clock)&&(isegment < nsegs)) {
//cout<<"loop: iseg = "<<isegment<<"; t_subgoal = "<<t_subgoal<<endl;
isegment++;
if (isegment > nsegs - 1) {
//last point
trajectory_point_to = trajectory.points[nsegs];
for (int i = 0; i < 7; i++) {
qvec_new[i] = trajectory_point_to.positions[i];
}
cout << "iseg>nsegs" << endl;
return false;
}
trajectory_point_to = trajectory.points[isegment + 1];
t_subgoal = trajectory_point_to.time_from_start.toSec();
}
//cout<<"t_subgoal = "<<t_subgoal<<endl;
//here if have a valid segment:
for (int i = 0; i < 7; i++) {
qvec_to[i] = trajectory_point_to.positions[i];
}
delta_qvec = qvec_to - qvec_prev; //this far to go until next node;
double delta_time = t_subgoal - traj_clock;
if (delta_time < dt_traj) delta_time = dt_traj;
dqvec = delta_qvec * dt_traj / delta_time;
qvec_new = qvec_prev + dqvec;
return true;
}
void inPointCallback(const geometry_msgs::Point& pt_msg) {
Eigen::Affine3d des_gripper1_wrt_base;
g_des_point(0) = pt_msg.x;
g_des_point(1) = pt_msg.y;
g_des_point(2) = pt_msg.z;
cout << "received des point = " << g_des_point.transpose() << endl;
g_des_gripper_affine1.translation() = g_des_point;
//convert this to base coords:
g_des_gripper1_wrt_base = g_affine_lcamera_to_psm_one.inverse() * g_des_gripper_affine1;
//try computing IK:
if (g_ik_solver.ik_solve(g_des_gripper1_wrt_base)) {
ROS_INFO("found IK soln");
g_got_new_pose = true;
g_q_vec1_start = g_q_vec1_goal;
g_q_vec1_goal = g_ik_solver.get_soln();
cout << g_q_vec1_goal.transpose() << endl;
g_trajectory_point1 = g_trajectory_point2; //former goal is new start
for (int i = 0; i < 6; i++) {
g_trajectory_point2.positions[i] = g_q_vec1_goal[i]; //leave psm2 angles alone; just update psm1 goal angles
}
//gripper_affines_wrt_camera.push_back(affine_gripper_frame_wrt_camera_frame_);
// 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[0] = g_des_trajectory.points[1]; //former goal is new start
g_des_trajectory.points[1] = g_trajectory_point2;
// copy traj to goal:
g_goal.trajectory = g_des_trajectory;
} else {
ROS_WARN("NO IK SOLN FOUND");
}
}
int main(int argc, char** argv) {
ros::init(argc, argv, "test_cart_path_planner_lib");
ros::NodeHandle nh; //standard ros node handle
ROS_INFO("instantiating an ArmMotionInterface: ");
ArmMotionInterface armMotionInterface(&nh);
//can do this, if needed: A = cartTrajPlanner.get_fk_Affine_from_qvec(Vectorq7x1 q_vec)
cout << "instantiating a traj streamer" << endl; // enter 1:";
//cin>>ans;
// let the streamer be owned by "main"
Baxter_traj_streamer baxter_traj_streamer(&nh); //instantiate a Baxter_traj_streamer object and pass in pointer to nodehandle for constructor to use
// warm up the joint-state callbacks;
cout << "warming up callbacks..." << endl;
for (int i = 0; i < 100; i++) {
ros::spinOnce();
//cout<<"spin "<<i<<endl;
ros::Duration(0.01).sleep();
}
cout << "getting current right-arm pose:" << endl;
g_q_vec_right_arm[0] = 1000;
while (fabs(g_q_vec_right_arm[0]) > 3) { // keep trying until see viable value populated by fnc
g_q_vec_right_arm = baxter_traj_streamer.get_qvec_right_arm();
ros::spinOnce();
ros::Duration(0.01).sleep();
}
cout << "r_arm state:" << g_q_vec_right_arm.transpose() << endl;
//let's leave this in main:
ROS_INFO("instantiating an action client of trajActionServer");
actionlib::SimpleActionClient<baxter_traj_streamer::trajAction> traj_streamer_action_client("trajActionServer", true);
// attempt to connect to the server:
ROS_INFO("waiting for server trajActionServer: ");
bool server_exists = false;
while ((!server_exists)&&(ros::ok())) {
server_exists = traj_streamer_action_client.waitForServer(ros::Duration(5.0)); // wait for up to 5 seconds
ros::spinOnce();
ros::Duration(0.5).sleep();
ROS_INFO("retrying...");
}
// something odd in above: does not seem to wait for 5 seconds, but returns rapidly if server not running
// if (!server_exists) {
// ROS_WARN("could not connect to server; will wait forever");
// return 0; // bail out; optionally, could print a warning message and retry
//}
//server_exists = action_client.waitForServer(); //wait forever
ROS_INFO("connected to action server"); // if here, then we connected to the server;
bool unpackok;
Eigen::Vector3d delta_p;
bool is_valid;
bool finished_before_timeout;
baxter_traj_streamer::trajGoal goal;
Vectorq7x1 q_vec_goal_vq7; //goal pose in joint space, expressed as a 7x1 vector
moveit_msgs::DisplayTrajectory display_trajectory;
ros::Publisher display_traj_pub = nh.advertise<moveit_msgs::DisplayTrajectory>("/preview_traj", 1, true);
// start servicing requests:
while (ros::ok()) {
g_q_vec_right_arm = baxter_traj_streamer.get_qvec_right_arm(); // update the current sensed arm angles
//decide if should start processing a new request:
if (armMotionInterface.newRqst()&&!armMotionInterface.isBusy()) {
armMotionInterface.setNewRqst(false); //g_received_new_request_=false; // reset trigger to receive a new request
armMotionInterface.setIsBusy(true); //g_busy_working_on_a_request_= true; // begin processing new request
}
if (armMotionInterface.isBusy()) {
switch (armMotionInterface.get_cmd_mode()) {
case ARM_TEST_MODE:
ROS_INFO("responding to request TEST_MODE: ");
cout << "unpacking q_vec_start from request message..." << endl;
unpackok = armMotionInterface.unpack_qstart();
if (unpackok) {
cout << "start qvec rqst: " << armMotionInterface.get_start_qvec().transpose() << endl;
}
unpackok = armMotionInterface.unpack_qend();
if (unpackok) {
cout << "end qvec rqst: " << armMotionInterface.get_end_qvec().transpose() << endl;
}
//reply w/ populating q_vec_start in response:
armMotionInterface.setIsBusy(false);
break;
case ARM_PLAN_JSPACE_PATH_CURRENT_TO_QGOAL:
ROS_INFO("case ARM_PLAN_JSPACE_PATH_CURRENT_TO_QGOAL");
//get the start q_vec = current arm pose:
cout << "getting current right-arm pose:" << endl;
g_q_vec_right_arm[0] = 1000; //Vectorq7x1 g_q_vec_right_arm;
while (fabs(g_q_vec_right_arm[0]) > 3) { // keep trying until see viable value populated by fnc
g_q_vec_right_arm = baxter_traj_streamer.get_qvec_right_arm();
ros::spinOnce();
ros::Duration(0.01).sleep();
}
//get the goal q_vec, per request message:
armMotionInterface.unpack_qend();
q_vec_goal_vq7= armMotionInterface.get_end_qvec(); //need to convert from VectorXd to Vectorq7x1
is_valid = armMotionInterface.plan_jspace_path_qstart_to_qend(g_q_vec_right_arm,q_vec_goal_vq7);
if (is_valid) ROS_INFO("computed valid path");
else ROS_INFO("no valid path found");
armMotionInterface.setIsBusy(false);
break;
case ARM_PLAN_PATH_CURRENT_TO_PRE_POSE:
ROS_INFO("responding to request PLAN_PATH_CURRENT_TO_PRE_POSE");
armMotionInterface.plan_path_to_predefined_pre_pose();
armMotionInterface.setIsBusy(false);
break;
case ARM_PLAN_PATH_CURRENT_TO_GOAL_POSE:
cout << "getting current right-arm pose:" << endl;
g_q_vec_right_arm[0] = 1000;
while (fabs(g_q_vec_right_arm[0]) > 3) { // keep trying until see viable value populated by fnc
g_q_vec_right_arm = baxter_traj_streamer.get_qvec_right_arm();
ros::spinOnce();
ros::Duration(0.01).sleep();
}
//armMotionInterface.unpack_qstart(); // fills in q_vec_start_rqst_--no...use current arm pose
armMotionInterface.unpack_goal_pose(); //fills in a_flange_end_; should check return status
// bool plan_path_qstart_to_Agoal(Vectorq7x1 q_start); // provide q_start; assumes use of a_flange_end_ and fills in g_optimal_path
is_valid = armMotionInterface.plan_path_qstart_to_Agoal(g_q_vec_right_arm);
if (is_valid) ROS_INFO("computed valid path");
else ROS_INFO("no valid path found");
armMotionInterface.setIsBusy(false);
break;
case ARM_DISPLAY_TRAJECTORY:
cout << "getting current joint states:" << endl;
g_q_vec_right_arm[0] = 1000;
while (fabs(g_q_vec_right_arm[0]) > 3) { // keep trying until see viable value populated by fnc
g_q_vec_right_arm = baxter_traj_streamer.get_qvec_right_arm();
ros::spinOnce();
ros::Duration(0.01).sleep();
}
//if here, then joint_states_ is valid
//populate the moveit message to preview the trajectory plan;
//display_trajectory.model_id = "robot_description"; // not sure what goes here; "baxter"??
//declare joint_states_ as start state of display
display_trajectory.trajectory_start.joint_state = baxter_traj_streamer.get_joint_states(); // get the joint states from Baxter;
baxter_traj_streamer.stuff_trajectory(g_optimal_path, g_des_trajectory);//convert planned path to trajectory
display_trajectory.trajectory.resize(1); //display only 1 trajectory
display_trajectory.trajectory[0].joint_trajectory = g_des_trajectory; //the one we care about
display_traj_pub.publish(display_trajectory); // and publish this to topic "/preview_traj"
armMotionInterface.setIsBusy(false);
break;
case ARM_DESCEND_20CM:
ROS_INFO("responding to request DESCEND_20CM");
delta_p << 0, 0, -0.2; //specify delta-p for a 20cm descent
// find a joint-space path to do this, holding R fixed
cout << "getting current right-arm pose:" << endl;
g_q_vec_right_arm[0] = 1000;
while (fabs(g_q_vec_right_arm[0]) > 3) { // keep trying until see viable value populated by fnc
g_q_vec_right_arm = baxter_traj_streamer.get_qvec_right_arm();
ros::spinOnce();
ros::Duration(0.01).sleep();
}
is_valid = armMotionInterface.plan_cartesian_delta_p(g_q_vec_right_arm, delta_p);
armMotionInterface.setPathValid(is_valid);
armMotionInterface.incrementPathID();
armMotionInterface.setIsBusy(false);
break;
case ARM_DEPART_20CM:
ROS_INFO("responding to request ARM_DEPART_20CM");
delta_p << 0, 0, 0.2; //specify delta-p for a 20cm descent
// find a joint-space path to do this, holding R fixed
cout << "getting current right-arm pose:" << endl;
g_q_vec_right_arm[0] = 1000;
while (fabs(g_q_vec_right_arm[0]) > 3) { // keep trying until see viable value populated by fnc
g_q_vec_right_arm = baxter_traj_streamer.get_qvec_right_arm();
ros::spinOnce();
ros::Duration(0.01).sleep();
}
is_valid = armMotionInterface.plan_cartesian_delta_p(g_q_vec_right_arm, delta_p);
armMotionInterface.setPathValid(is_valid);
armMotionInterface.incrementPathID();
armMotionInterface.setIsBusy(false);
break;
//case DEPART_20CM:
//case CART_MOVE_DELTA_P: unpack_delta_p()...
//case PLAN_PATH_QSTART_TO_ADES:
//case PLAN_PATH_QSTART_TO_QGOAL:
//case PLAN_PATH_ASTART_TO_QGOAL:
case ARM_EXECUTE_PLANNED_PATH: //assumes there is a valid planned path in g_optimal_path
ROS_INFO("responding to request EXECUTE_PLANNED_PATH");
if (armMotionInterface.getPathValid()) {
// convert path to a trajectory:
baxter_traj_streamer.stuff_trajectory(g_optimal_path, g_des_trajectory); //convert from vector of 7dof poses to trajectory message
goal.trajectory = g_des_trajectory;
ROS_INFO("sending action request to traj streamer node");
traj_streamer_action_client.sendGoal(goal, &doneCb); // we could also name additional callback functions here, if desired
// action_client.sendGoal(goal, &doneCb, &activeCb, &feedbackCb); //e.g., like this
finished_before_timeout = traj_streamer_action_client.waitForResult(ros::Duration(20.0));
if (!finished_before_timeout) {
ROS_WARN("EXECUTE_PLANNED_PATH: giving up waiting on result");
// should set status in service request here...
} else {
ROS_INFO("finished before timeout");
// should set status in service request here...
}
} else {
ROS_WARN("requested move without valid path! doing nothing");
}
armMotionInterface.setIsBusy(false);
break;
default:
ROS_WARN("this command mode is not defined: %d", armMotionInterface.get_cmd_mode());
//clean up/terminate:
armMotionInterface.setIsBusy(false);
}
}
ros::spinOnce();
//cout<<"main loop..."<<endl;
ros::Duration(0.1).sleep(); //don't consume much cpu time if not actively working on a command
}
return 0;
}
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;
}
int main(int argc, char **argv) {
ros::init(argc, argv, "baxter_kinematics_test_main");
ros::NodeHandle nh;
ros::Subscriber joint_state_sub = nh.subscribe("robot/joint_states", 1, jointStatesCb);
q_vec_right_arm<< 0,0,0,0,0,0,0;
q_in << 0,0,0,0,0,0,0;
//q_in << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6;
tf::TransformListener tfListener;
tf::StampedTransform tfResult;
// set up and initialize joint publishers:
ros::Publisher pub_right, pub_left; //Create a 'publisher' object
baxter_core_msgs::JointCommand right_cmd,left_cmd; // define instances of these message types, to control arms
left_cmd.mode = 1; // set the command modes to "position"
right_cmd.mode = 1;
// define the joint angles 0-6 to be right arm, from shoulder out to wrist;
right_cmd.names.push_back("right_s0");
right_cmd.names.push_back("right_s1");
right_cmd.names.push_back("right_e0");
right_cmd.names.push_back("right_e1");
right_cmd.names.push_back("right_w0");
right_cmd.names.push_back("right_w1");
right_cmd.names.push_back("right_w2");
// same order for left arm
left_cmd.names.push_back("left_s0");
left_cmd.names.push_back("left_s1");
left_cmd.names.push_back("left_e0");
left_cmd.names.push_back("left_e1");
left_cmd.names.push_back("left_w0");
left_cmd.names.push_back("left_w1");
left_cmd.names.push_back("left_w2");
// do push-backs to establish desired vector size with valid joint angles
for (int i=0;i<7;i++) {
right_cmd.command.push_back(0.0); // start commanding 0 angle for right-arm 7 joints
left_cmd.command.push_back(0.0); // start commanding 0 angle for left-arm 7 joints
}
//set up the publisher to publish to /robot/limb/right/joint_command with a queue size of 1
pub_right = nh.advertise<baxter_core_msgs::JointCommand>("/robot/limb/right/joint_command", 1);
//do same for left limb:
pub_left = nh.advertise<baxter_core_msgs::JointCommand>("/robot/limb/left/joint_command", 1);
bool tferr=true;
ROS_INFO("waiting for tf between base and right_hand...");
while (tferr) {
tferr=false;
try {
//try to lookup transform from target frame "odom" to source frame "map"
//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("right_arm_mount","right_hand", ros::Time(0), tfResult);
} catch(tf::TransformException &exception) {
ROS_ERROR("%s", exception.what());
tferr=true;
ros::Duration(0.5).sleep(); // sleep for half a second
ros::spinOnce();
}
}
ROS_INFO("tf is good");
//right_upper_shoulder is located at bottom of first shoulder jnt
// from now on, tfListener will keep track of transforms
tfListener.lookupTransform("right_arm_mount", "right_lower_shoulder", ros::Time(0), tfResult);
tf::Vector3 pos = tfResult.getOrigin();
ROS_INFO("shoulder x,y, z = %f, %f, %f",pos[0],pos[1],pos[2]);
tfListener.lookupTransform("right_arm_mount","right_lower_elbow", ros::Time(0), tfResult);
pos = tfResult.getOrigin();
ROS_INFO("elbow x,y, z = %f, %f, %f",pos[0],pos[1],pos[2]);
tfListener.lookupTransform("right_arm_mount","right_lower_forearm", ros::Time(0), tfResult);
pos = tfResult.getOrigin();
ROS_INFO("wrist x,y, z = %f, %f, %f",pos[0],pos[1],pos[2]);
tfListener.lookupTransform("right_arm_mount","right_hand", ros::Time(0), tfResult);
pos = tfResult.getOrigin();
ROS_INFO("hand x,y, z = %f, %f, %f",pos[0],pos[1],pos[2]);
Baxter_fwd_solver baxter_fwd_solver; //instantiate a forward-kinematics solver
Baxter_IK_solver baxter_IK_solver; // instantiate an IK solver
//Baxter_IK_solver ik_solver;
Eigen::Affine3d A_fwd_DH;
Eigen::Matrix4d A_wrist,A_elbow,A_shoulder,A_flange;
Eigen::Matrix3d R_hand;
std::cout << "==== Test for Baxter kinematics solver ====" << std::endl;
Eigen::Matrix3d R_flange_wrt_right_arm_mount;
Eigen::Vector3d wrist_pt_wrt_right_arm_frame1;
Eigen::Vector3d w_err;
int ans;
int nsolns;
bool valid;
Vectorq7x1 q_vec_display,q_precise,q_soln_w_wrist_precise;
std::vector<Vectorq7x1> q_solns;
// try new fnc:
//int Baxter_IK_solver::ik_solve_approx_wrt_torso_given_qs0(Eigen::Affine3d const& desired_hand_pose_wrt_torso,double q_s0, std::vector<Vectorq7x1> &q_solns) {
q_snapshot = q_vec_right_arm;
cout<<"qvec right arm: "<<endl;
cout<< q_snapshot.transpose() <<endl;
// from this test pose, establish a viable hand frame:
Eigen::Affine3d Affine_flange_wrt_torso = baxter_fwd_solver.fwd_kin_flange_wrt_torso_solve(q_snapshot); //fwd_kin_solve
Eigen::Affine3d Affine_flange_wrt_arm_mount =
baxter_fwd_solver.transform_affine_from_torso_frame_to_arm_mount_frame(Affine_flange_wrt_torso);
R_flange_wrt_right_arm_mount = Affine_flange_wrt_arm_mount.linear();
nsolns = baxter_IK_solver.ik_solve_approx_wrt_torso_given_qs0(Affine_flange_wrt_torso,q_snapshot[0],q_solns);
cout<<"ik_solve_approx_wrt_torso_given_qs0 found nsolns = "<<nsolns<<endl;
for (int i=0;i<nsolns;i++) {
cout<<q_solns[i].transpose()<<endl;
}
Vectorq7x1 q_approx;
Vectorq7x1 q_7dof_precise;
cout<<"result of new fnc improve_7dof_soln:"<<endl;
for (int i=0;i<nsolns;i++) {
q_approx = q_solns[i];
valid = baxter_IK_solver.improve_7dof_soln(Affine_flange_wrt_arm_mount, q_approx, q_7dof_precise);
cout<<q_7dof_precise.transpose()<<endl;
}
//q_precise = q_solns[0]; // pick one and improve on it;
// need to do this for each soln, e.g. 4 solns per q_s0;
//double w_err_norm= baxter_IK_solver.precise_soln_q123(Affine_flange_wrt_arm_mount,q_solns[0], q_precise);
//cout<<"q123 soln after Jacobian iterations: "<<endl;
//cout<<q_precise.transpose()<<endl;
//correspondingly, do this for each of the above improved q123 solns
//baxter_IK_solver.update_spherical_wrist(q_precise,R_flange_wrt_right_arm_mount, q_soln_w_wrist_precise);
//cout<<"soln after update wrist: "<<endl;
//cout<<q_soln_w_wrist_precise.transpose()<<endl;
//valid = baxter_IK_solver.improve_7dof_soln(Affine_flange_wrt_arm_mount, q_approx, q_7dof_precise);
//cout<<"result of new fnc improve_7dof_soln:"<<endl;
//cout<<q_7dof_precise.transpose()<<endl;
//baxter_IK_solver.solve_spherical_wrist(q_precise,R_flange_wrt_right_arm_mount, q_soln_w_wrist_precise);
//cout<<"solns after Jacobian iterations: "<<endl;
//for (int i=0;i<q_solns_w_wrist_precise.size();i++) {
// cout<<q_solns_w_wrist_precise[i].transpose()<<endl;
//}
while (ros::ok())
{
cout<<"enter 1 to initiate: "; //pause here to allow user to move joint angles to desired test pose
cin>>ans;
for (int i=0;i<10;i++) { // spin to get current joint angles
ros::spinOnce();
ros::Duration(0.1).sleep(); //sleep for 2 seconds
}
q_snapshot = q_vec_right_arm;
cout<<"qvec right arm: "<<endl;
cout<< q_snapshot.transpose() <<endl;
// from this test pose, establish a viable hand frame:
A_fwd_DH = baxter_fwd_solver.fwd_kin_flange_wrt_r_arm_mount_solve(q_snapshot); //fwd_kin_solve
cout<<"TRYING NEW FNC ik_solve_approx"<<endl;
nsolns = baxter_IK_solver.ik_solve_approx(A_fwd_DH,q_solns);
cout<<"num solns: "<<nsolns<<endl;
cout<<"commanding these solutions..."<<endl;
for (int isoln=0; isoln<nsolns; isoln++) {
q_vec_display= q_solns[isoln];
for (int j=0;j<7;j++) {
right_cmd.command[j] = q_vec_display[j];
}
pub_right.publish(right_cmd); //publish jnt cmds to right arm
cout<<"soln: "<<q_vec_display.transpose()<<endl;
ros::Duration(2).sleep(); //sleep for 2 seconds
ros::spinOnce();
}
ros::Duration(2).sleep(); //sleep for 2 seconds
ros::spinOnce();
}
return 0;
}
