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.");
}
}
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();
}
void wave()
{
ros::AsyncSpinner spinner(0);
spinner.start();
// reset_arms();
sleep(1);
//Create a series of stored plans, to be executed one by one in a blocking queue
//Get current state of robot->create plan to next state->set new start state to that one->repeat
robot_state::RobotStatePtr curr_state=right_arm.getCurrentState();
std::map<std::string, double> r_jm;
r_jm.insert(std::pair<std::string,double>("right_s0", -0.2868));
r_jm.insert(std::pair<std::string,double>("right_s1", -0.6756));
r_jm.insert(std::pair<std::string,double>("right_e0", 1.1900));
r_jm.insert(std::pair<std::string,double>("right_e1", 1.94));
r_jm.insert(std::pair<std::string,double>("right_w0", -0.67));
r_jm.insert(std::pair<std::string,double>("right_w1", -1.5707));
r_jm.insert(std::pair<std::string,double>("right_w2", 0.50));
moveit::planning_interface::MoveGroup::Plan r_ap0;
right_arm.setJointValueTarget(r_jm);+
right_arm.plan(r_ap0);
curr_state->setStateValues(r_jm);
right_arm.setStartState(*curr_state);
r_jm.clear();
r_jm.insert(std::pair<std::string,double>("right_s0", -0.2868));
r_jm.insert(std::pair<std::string,double>("right_s1", -0.6756));
r_jm.insert(std::pair<std::string,double>("right_e0", 1.1900));
r_jm.insert(std::pair<std::string,double>("right_e1", 1.94));
r_jm.insert(std::pair<std::string,double>("right_w0", -1.5467));
r_jm.insert(std::pair<std::string,double>("right_w1", -1.5707));
r_jm.insert(std::pair<std::string,double>("right_w2", 0.50));
moveit::planning_interface::MoveGroup::Plan r_ap1;
right_arm.setJointValueTarget(r_jm);
right_arm.plan(r_ap1);
curr_state->setStateValues(r_jm);
right_arm.setStartState(*curr_state);
r_jm.clear();
r_jm.insert(std::pair<std::string,double>("right_s0", -0.2868));
r_jm.insert(std::pair<std::string,double>("right_s1", -0.6756));
r_jm.insert(std::pair<std::string,double>("right_e0", 1.1900));
r_jm.insert(std::pair<std::string,double>("right_e1", 1.94));
r_jm.insert(std::pair<std::string,double>("right_w0", -0.2062));
r_jm.insert(std::pair<std::string,double>("right_w1", -1.5707));
r_jm.insert(std::pair<std::string,double>("right_w2", 0.50));
moveit::planning_interface::MoveGroup::Plan r_ap2;
right_arm.setJointValueTarget(r_jm);
right_arm.plan(r_ap2);
curr_state->setStateValues(r_jm);
right_arm.setStartState(*curr_state);
r_jm.clear();
r_jm.insert(std::pair<std::string,double>("right_s0", -0.2868));
r_jm.insert(std::pair<std::string,double>("right_s1", -0.6756));
r_jm.insert(std::pair<std::string,double>("right_e0", 1.1900));
r_jm.insert(std::pair<std::string,double>("right_e1", 1.94));
r_jm.insert(std::pair<std::string,double>("right_w0", -1.5467));
r_jm.insert(std::pair<std::string,double>("right_w1", -1.5707));
r_jm.insert(std::pair<std::string,double>("right_w2", 0.50));
moveit::planning_interface::MoveGroup::Plan r_ap3;
right_arm.setJointValueTarget(r_jm);
right_arm.plan(r_ap3);
// curr_state->setStateValues(r_jm);
// right_arm.setStartState(*curr_state);
// r_jm.clear();
// r_jm.insert(std::pair<std::string,double>("right_s0", -0.2868));
// r_jm.insert(std::pair<std::string,double>("right_s1", -0.6756));
// r_jm.insert(std::pair<std::string,double>("right_e0", 1.1900));
// r_jm.insert(std::pair<std::string,double>("right_e1", 1.94));
// r_jm.insert(std::pair<std::string,double>("right_w0", -0.2062));
// r_jm.insert(std::pair<std::string,double>("right_w1", -1.5707));
// r_jm.insert(std::pair<std::string,double>("right_w2", 0.50));
// moveit::planning_interface::MoveGroup::Plan r_ap4;
// right_arm.setJointValueTarget(r_jm);
// right_arm.plan(r_ap4);
right_arm.execute(r_ap0);
right_arm.execute(r_ap1);
right_arm.execute(r_ap2);
right_arm.execute(r_ap3);
// right_arm.execute(r_ap4);
spinner.stop();
}
