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.");
  }
}
Ejemplo n.º 2
0
    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();
}
Ejemplo n.º 4
0
    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();

    }