/*! \brief Given two robot states, determine how many interpolation steps there
* should be. The delta step size is based on the RPY resolution of the object
* pose and the XYZ spatial resolution.
*/
int RobotState::numInterpSteps(const RobotState& start, const RobotState& end){
ContObjectState start_obj = start.getObjectStateRelBody();
ContObjectState end_obj = end.getObjectStateRelBody();
ContBaseState start_base = start.base_state();
ContBaseState end_base = end.base_state();
double droll = shortest_angular_distance(start_obj.roll(), end_obj.roll());
double dpitch = shortest_angular_distance(start_obj.pitch(), end_obj.pitch());
double dyaw = shortest_angular_distance(start_obj.yaw(), end_obj.yaw());
double d_rot = max(fabs(droll), fabs(dpitch));
double dbase_theta = shortest_angular_distance(start_base.theta(),
end_base.theta());
ROS_DEBUG_NAMED(MPRIM_LOG, "droll %f, dpitch %f, dyaw %f, dbase_theta %f",
droll, dpitch, dyaw, dbase_theta);
d_rot = max(d_rot, fabs(dyaw));
d_rot = max(d_rot, fabs(dbase_theta));
double d_object = ContObjectState::distance(start_obj, end_obj);
double d_base = ContBaseState::distance(ContBaseState(start.base_state()),
ContBaseState(end.base_state()));
ROS_DEBUG_NAMED(MPRIM_LOG, "dobject %f, dbase %f", d_object, d_base);
double d_dist = max(d_object, d_base);
int rot_steps = static_cast<int>(d_rot/ContObjectState::getRPYResolution());
int dist_steps = static_cast<int>(d_dist/ContBaseState::getXYZResolution());
ROS_DEBUG_NAMED(MPRIM_LOG, "rot steps %d, dist_steps %d", rot_steps, dist_steps);
int num_interp_steps = max(rot_steps, dist_steps);
return num_interp_steps;
}
/*! \brief Given two robot states, we interpolate all steps in between them. The
* delta step size is based on the RPY resolution of the object pose and the XYZ
* resolution of the base. This returns a vector of RobotStates, which are
* discretized to the grid (meaning if the arms move a lot, but the base barely
* moves, the base discrete values will likely stay the same). This determines
* the number of interpolation steps based on which part of the robot moves more
* (arms or base).
* TODO need to test this a lot more
*/
bool RobotState::workspaceInterpolate(const RobotState& start, const RobotState& end,
vector<RobotState>* interp_steps){
ContObjectState start_obj = start.getObjectStateRelBody();
ContObjectState end_obj = end.getObjectStateRelBody();
ContBaseState start_base = start.base_state();
ContBaseState end_base = end.base_state();
int num_interp_steps = numInterpSteps(start, end);
vector<ContObjectState> interp_obj_steps;
vector<ContBaseState> interp_base_steps;
ROS_DEBUG_NAMED(MPRIM_LOG, "start obj");
start_obj.printToDebug(MPRIM_LOG);
ROS_DEBUG_NAMED(MPRIM_LOG, "end obj");
end_obj.printToDebug(MPRIM_LOG);
interp_obj_steps = ContObjectState::interpolate(start_obj, end_obj,
num_interp_steps);
interp_base_steps = ContBaseState::interpolate(start_base, end_base,
num_interp_steps);
assert(interp_obj_steps.size() == interp_base_steps.size());
ROS_DEBUG_NAMED(MPRIM_LOG, "size of returned interp %lu", interp_obj_steps.size());
// should at least return the same start and end poses
if (num_interp_steps < 2){
assert(interp_obj_steps.size() == 2);
} else {
assert(interp_obj_steps.size() == static_cast<size_t>(num_interp_steps));
}
for (size_t i=0; i < interp_obj_steps.size(); i++){
interp_obj_steps[i].printToDebug(MPRIM_LOG);
RobotState seed(interp_base_steps[i], start.right_arm(), start.left_arm());
RobotPosePtr new_robot_state;
if (!computeRobotPose(interp_obj_steps[i], seed, new_robot_state)){
return false;
}
interp_steps->push_back(*new_robot_state);
}
return true;
}
FullBodyState PathPostProcessor::createFBState(const RobotState& robot){
vector<double> l_arm, r_arm, base;
vector<double> obj(6,0);
robot.right_arm().getAngles(&r_arm);
robot.left_arm().getAngles(&l_arm);
ContBaseState c_base = robot.base_state();
c_base.getValues(&base);
FullBodyState state;
state.left_arm = l_arm;
state.right_arm = r_arm;
state.base = base;
ContObjectState obj_state = robot.getObjectStateRelMap();
obj[0] = obj_state.x();
obj[1] = obj_state.y();
obj[2] = obj_state.z();
obj[3] = obj_state.roll();
obj[4] = obj_state.pitch();
obj[5] = obj_state.yaw();
state.obj = obj;
return state;
}
// this is a bit weird at the moment, but we use the arm angles as seed angles
// disc_obj_state is in body frame : Torso_lift_link?
bool RobotState::computeRobotPose(const DiscObjectState& disc_obj_state,
const RobotState& seed_robot_pose,
RobotPosePtr& new_robot_pose,
bool free_angle_search){
ContObjectState obj_state = disc_obj_state.getContObjectState();
RightContArmState seed_r_arm = seed_robot_pose.right_arm();
LeftContArmState seed_l_arm = seed_robot_pose.left_arm();
KDL::Frame obj_frame;
obj_frame.p.x(obj_state.x());
obj_frame.p.y(obj_state.y());
obj_frame.p.z(obj_state.z());
obj_frame.M = KDL::Rotation::RPY(obj_state.roll(),
obj_state.pitch(),
obj_state.yaw());
KDL::Frame r_obj_to_wrist_offset = seed_robot_pose.right_arm().getObjectOffset();
KDL::Frame l_obj_to_wrist_offset = seed_robot_pose.left_arm().getObjectOffset();
KDL::Frame r_wrist_frame = obj_frame * r_obj_to_wrist_offset;
KDL::Frame l_wrist_frame = obj_frame * l_obj_to_wrist_offset;
// store the seed angles as the actual angles in case we don't compute for
// the other arm. otherwise, computing a new robot pose would reset the
// unused arm to angles of 0
vector<double> r_seed(7,0), r_angles(7,0), l_seed(7,0), l_angles(7,0);
seed_r_arm.getAngles(&r_seed);
r_angles = r_seed;
seed_l_arm.getAngles(&l_seed);
l_angles = l_seed;
ik_calls++;
struct timeval tv_b;
struct timeval tv_a;
gettimeofday(&tv_b, NULL);
double before = tv_b.tv_usec + (tv_b.tv_sec * 1000000);
gettimeofday(&tv_a, NULL);
// decide which arms we need to run IK for
bool use_right_arm = (m_planning_mode == PlanningModes::RIGHT_ARM ||
m_planning_mode == PlanningModes::DUAL_ARM ||
m_planning_mode == PlanningModes::RIGHT_ARM_MOBILE ||
m_planning_mode == PlanningModes::DUAL_ARM_MOBILE);
bool use_left_arm = (m_planning_mode == PlanningModes::LEFT_ARM ||
m_planning_mode == PlanningModes::DUAL_ARM ||
m_planning_mode == PlanningModes::LEFT_ARM_MOBILE ||
m_planning_mode == PlanningModes::DUAL_ARM_MOBILE);
if (!(use_right_arm || use_left_arm)){
ROS_ERROR("what! not using any arm for IK??");
}
#ifdef USE_KDL_SOLVER
if (use_right_arm) {
SBPLArmModelPtr arm_model = seed_r_arm.getArmModel();
bool ik_success = arm_model->computeFastIK(r_wrist_frame, r_seed, r_angles);
if (!ik_success) {
ROS_DEBUG_NAMED(MPRIM_LOG, "IK failed for right arm");
return false;
}
}
if (use_left_arm) {
SBPLArmModelPtr arm_model = seed_l_arm.getArmModel();
bool ik_success = arm_model->computeFastIK(l_wrist_frame, l_seed, l_angles);
if (!ik_success){
ROS_DEBUG_NAMED(MPRIM_LOG, "IK failed for left arm");
return false;
}
}
#endif
#ifdef USE_IKFAST_SOLVER
if (use_right_arm){
double r_free_angle = r_seed[Joints::UPPER_ARM_ROLL];
if (!m_ikfast_solver.ikRightArm(r_wrist_frame, r_free_angle, &r_angles)){
return false;
}
}
if (use_left_arm){
double l_free_angle = l_seed[Joints::UPPER_ARM_ROLL];
if (!m_ikfast_solver.ikLeftArm(l_wrist_frame, l_free_angle, &l_angles)){
return false;
}
}
#endif
double after = tv_a.tv_usec + (tv_a.tv_sec * 1000000);
ik_time += after - before;
new_robot_pose = make_shared<RobotState>(seed_robot_pose.base_state(),
RightContArmState(r_angles),
LeftContArmState(l_angles));
return true;
}