bool Actuator::moveToPose(geometry_msgs::Pose goalPose, char armName){
bool isPlanningSuccess;
moveit::planning_interface::MoveGroup::Plan my_plan;
rightArm.setStartState(*rightArm.getCurrentState());
std::string planningArm = "left_gripper";
if(armName == amazon::MoveToGoal::RIGHT_ARM){
planningArm = "right_gripper";
}
else if(armName != amazon::MoveToGoal::LEFT_ARM){
ROS_WARN("Unknown arm received. Using left arm");
}
rightArm.setPoseTarget(goalPose, planningArm.c_str());
// call the planner to compute the plan and visualize it
isPlanningSuccess = rightArm.plan(my_plan);
/* ***************** FOR DEBUGGING ***************/
ROS_INFO("Visualizing plan %s", isPlanningSuccess?"":"FAILED");
if(isPlanningSuccess){
//rightArm.execute(my_plan);
return true;
}
else{
return false;
}
}
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 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();
}
void leapmotionCallback(const leap_motion::leapros2::ConstPtr& dataHand)
{
dataHand_=(*dataHand);
//ROS_INFO("Visualizing plan 1 (pose goal) %s",success?"":"FAILED");
// Both limits for x,y,z to avoid small changes
Updifferencex=dataLastHand_.palmpos.x+0.2;
Downdifferencex=dataLastHand_.palmpos.x-0.2;
Updifferencez=dataLastHand_.palmpos.z+0.3;
Downdifferencez=dataLastHand_.palmpos.z-0.2;
Updifferencey=dataLastHand_.palmpos.y+0.5;
Downdifferencey=dataLastHand_.palmpos.y-0.5;
//joint_msg_leap.header.stamp = ros::Time::now();
if ((dataHand_.palmpos.x<Downdifferencex)||(dataHand_.palmpos.x>Updifferencex)||(dataHand_.palmpos.y<Downdifferencey)||(dataHand_.palmpos.y>Updifferencey)||(dataHand_.palmpos.z<Downdifferencez)||(dataHand_.palmpos.z>Updifferencez))
{
ros::AsyncSpinner spinner(1);
spinner.start();
//target_pose2.position.y +=(dataHand_.palmpos.x-dataLastHand_.palmpos.x)/100 ;
target_pose2.position.z +=(dataHand_.palmpos.y-dataLastHand_.palmpos.y)/100 ;
//target_pose2.position.x +=-(dataHand_.palmpos.z-dataLastHand_.palmpos.z)/100 ;
if(target_pose2.position.z>Uplimitez)target_pose2.position.z=Uplimitez;
if(target_pose2.position.z<Downlimitez)target_pose2.position.z=Downlimitez;
//target_pose2.position.z += 0.2
pgroup->setPoseTarget(target_pose2);
bool success = pgroup->plan(*pplan);
//pgroup->setStartStateToCurrentState();
//we change the initial state
//robot_state::RobotState start_state(*pgroup->getCurrentState());
//ROS_INFO("Visualizing plan 1 (pose goal) %s",success?"":"FAILED");
//target_pose2.position.x=dataHand_.palmpos.x-dataLastHand_.palmpos.x;
//target_pose2.position.y=dataHand_.palmpos.y-dataLastHand_.palmpos.y;
//target_pose2.position.z=dataHand_.palmpos.z-dataLastHand_.palmpos.z;*/
//ROS_INFO("Move ARM");
//sleep(5.0);
/*const robot_state::JointModelGroup *joint_model_group =
start_state.getJointModelGroup(pgroup->getName());
if(start_state.setFromIK(joint_model_group, target_pose2))
{
ROS_INFO("ik calculated");
sleep (1);
}/*
pgroup->setStartState(start_state);
spinner.stop();
//group.setPoseTarget(target_pose2);
/*moveit::planning_interface::MoveGroup::Plan my_plan;
bool success = group.plan(my_plan);
*/
}
else
{
//printf("Palmpos \n X: %f\n Y: %f\n Z: %f\n ",dataHand_.palmpos.x,dataHand_.palmpos.y,dataHand_.palmpos.z);
}
//save new value of the last position of the hand
dataLastHand_=(*dataHand);
//we print the position of all the hand
//}
}
