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