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