void moveArmsToHomePose(moveit::planning_interface::MoveGroup &arms)
{
std::map<std::string, double> target;
target["r_elbow_flex_joint"] = -2.12441;
// target["r_forearm_roll_joint"] = -1.4175;
target["r_shoulder_lift_joint"] = 1.24853;
target["r_shoulder_pan_joint"] = -1.5;
// target["r_upper_arm_roll_joint"] = -1.55669;
// target["r_wrist_flex_joint"] = -1.8417;
// target["r_wrist_roll_joint"] = 0.21436;
arms.setJointValueTarget(target);
while (!arms.move()){
ROS_INFO("Right arm motion failed. Retrying.");
}
target.clear();
target["l_elbow_flex_joint"] = -1.68339;
target["l_forearm_roll_joint"] = -1.73434;
target["l_shoulder_lift_joint"] = 1.24852;
target["l_shoulder_pan_joint"] = 0.06024;
target["l_upper_arm_roll_joint"] = 1.78907;
target["l_wrist_flex_joint"] = -0.0962141;
target["l_wrist_roll_joint"] = -0.0864407;
arms.setJointValueTarget(target);
while (!arms.move()){
ROS_INFO("Left arm motion failed. Retrying.");
}
}
bool ServiceWout::planAndMove()
{
if(!trigger){
return false;
}
trigger = false;
ROS_INFO("Setting target pose...");
ROS_INFO("Reference frame: %s", group->getEndEffectorLink().c_str());
group->setPoseTarget(stored_pose);
ROS_INFO("Preparing to plan...");
moveit::planning_interface::MoveGroup::Plan my_plan;
group->plan(my_plan);
sleep(10.0);
ROS_INFO("Preparing to move...");
group->move();
return true;
}
void reset_arms(){
ros::AsyncSpinner spinner(0);
spinner.start();
std::map<std::string,double> jointmap;
jointmap.insert(std::pair<std::string,double>("left_s0", -0.08));
jointmap.insert(std::pair<std::string,double>("left_s1", -1.0));
jointmap.insert(std::pair<std::string,double>("left_e0", -1.19));
jointmap.insert(std::pair<std::string,double>("left_e1", 1.94));
jointmap.insert(std::pair<std::string,double>("left_w0", 0.67));
jointmap.insert(std::pair<std::string,double>("left_w1", 1.03));
jointmap.insert(std::pair<std::string,double>("left_w2", -0.50));
jointmap.insert(std::pair<std::string,double>("right_s0", 0.08));
jointmap.insert(std::pair<std::string,double>("right_s1", -1.0));
jointmap.insert(std::pair<std::string,double>("right_e0", 1.19));
jointmap.insert(std::pair<std::string,double>("right_e1", 1.94));
jointmap.insert(std::pair<std::string,double>("right_w0", -0.67));
jointmap.insert(std::pair<std::string,double>("right_w1", 1.03));
jointmap.insert(std::pair<std::string,double>("right_w2", 0.50));
both_arms.setJointValueTarget(jointmap);
both_arms.move();
spinner.stop();
}
void tiltHead(moveit::planning_interface::MoveGroup &head, double value)
{
head.setJointValueTarget("head_tilt_joint", value);
head.move();
}