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