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 HapticsPSM::check_collison(){
coll_res.clear();
psm_planning_scene->checkCollisionUnpadded(coll_req,coll_res,*group->getCurrentState(),
psm_planning_scene->getAllowedCollisionMatrix());
if (coll_res.collision)
{
//ROS_INFO("COLLIDING contact_point_count=%d",(int)coll_res.contact_count);
if (coll_res.contact_count > 0)
{
std_msgs::ColorRGBA color;
color.r = 1.0;
color.g = 0.0;
color.b = 0.2;
color.a = 0.8;
visualization_msgs::MarkerArray markers;
collision_detection::getCollisionMarkersFromContacts(markers,
group->getPlanningFrame().c_str(),
coll_res.contacts,
color,
ros::Duration(), // remain until deleted
0.002);
publishMarkers(markers);
}
return true;
}
else
{
//ROS_INFO("Not colliding");
// delete the old collision point markers
visualization_msgs::MarkerArray empty_marker_array;
publishMarkers(empty_marker_array);
return false;
}
}
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();
}