void Arm::sendArmToPoses(std::vector<std::vector<double> > &poses,
std::vector<double> move_times) {
geometry_msgs::Pose pose_msg;
btQuaternion quaternion;
geometry_msgs::Quaternion quaternion_msg;
std::vector<std::vector<double> > configurations(poses.size(),
std::vector<double>(7, 0));
std::vector<double> jnt_pos(7, 0), solution(7, 0);
for (unsigned int i = 0; i < poses.size(); i++) {
quaternion.setRPY(poses[i][3], poses[i][4], poses[i][5]);
tf::quaternionTFToMsg(quaternion, quaternion_msg);
pose_msg.position.x = poses[i][0];
pose_msg.position.y = poses[i][1];
pose_msg.position.z = poses[i][2];
pose_msg.orientation = quaternion_msg;
double start_time = ros::Time::now().toSec();
if (computeIK(pose_msg, jnt_pos, configurations[i]))
ROS_DEBUG(
"[sendArmToPoses] Computed IK solution in %0.3f seconds", ros::Duration(ros::Time::now().toSec() - start_time).toSec());
else
return;
}
sendArmToConfigurations(configurations, move_times);
}
bool Kinematics::computeIK(const string &arm,
const geometry_msgs::Pose &goal,
vector<double> &solution,
const bool avoid_collisions,
const int attempts,
const double timeout) {
// set default tip link to last link in arm chain
string tip_link = arm + "_arm_7_link";
return computeIK(arm, goal, tip_link, solution, avoid_collisions, attempts, timeout);
}
bool KDLRobotModel::computeIK(
const Eigen::Affine3d& pose,
const RobotState& start,
std::vector<RobotState>& solutions,
ik_option::IkOption option)
{
// NOTE: only returns one solution
RobotState solution;
if (computeIK(pose, start, solution)) {
solutions.push_back(solution);
}
return solutions.size() > 0;
}
bool Kinematics::computeIK(const string &arm,
const geometry_msgs::Pose &goal,
const string &tip_link,
vector<double> &solution,
const bool avoid_collisions,
const int attempts,
const double timeout) {
// use empty vector as default seed state
vector<double> seed_state;
return computeIK(arm, goal, tip_link, seed_state, solution, avoid_collisions, attempts, timeout);
}
bool Arm::sendArmToPose(double pose[6], double move_time, bool constraint_aware) {
btQuaternion quaternion;
geometry_msgs::Quaternion quaternion_msg;
quaternion.setRPY(pose[3], pose[4], pose[5]);
tf::quaternionTFToMsg(quaternion, quaternion_msg);
geometry_msgs::Pose pose_msg;
pose_msg.position.x = pose[0];
pose_msg.position.y = pose[1];
pose_msg.position.z = pose[2];
pose_msg.orientation = quaternion_msg;
std::vector<double> jnt_pos(7, 0), solution(7, 0);
pr2_controllers_msgs::JointTrajectoryControllerStateConstPtr state;
if(arm_name_.compare("right_arm") == 0)
state = ros::topic::waitForMessage<pr2_controllers_msgs::JointTrajectoryControllerState>("r_arm_controller/state", nh_, ros::Duration(2));
else
state = ros::topic::waitForMessage<pr2_controllers_msgs::JointTrajectoryControllerState>("l_arm_controller/state", nh_, ros::Duration(2));
if(state->actual.positions.size() == jnt_pos.size())
ROS_INFO("[arm] Received the joint angles fromthe arm controller topic.");
for(size_t i = 0; i < jnt_pos.size(); ++i)
jnt_pos[i] = state->actual.positions[i];
double start_time = ros::Time::now().toSec();
if(constraint_aware)
{
if (computeConstraintAwareIK(pose_msg, jnt_pos, solution)) {
ROS_INFO("[arm] Computed constraint aware IK solution in %0.3f seconds", ros::Duration(ros::Time::now().toSec() - start_time).toSec());
} else
return false;
}
else
{
if (computeIK(pose_msg, jnt_pos, solution)) {
ROS_INFO("[arm] Computed IK solution in %0.3f seconds", ros::Duration(ros::Time::now().toSec() - start_time).toSec());
} else
return false;
}
double configuration[7];
for (int i = 0; i < 7; i++)
configuration[i] = solution[i];
//sendArmToConfiguration(configuration, move_time);
sendArmToConfiguration(solution, move_time);
return true;
}
