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;
}
}
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();
}
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 HapticsPSM::compute_force_in_tip_frame(geometry_msgs::Wrench &wrench){
rot_mat6wrt0.setRPY(group->getCurrentRPY().at(0),
group->getCurrentRPY().at(1),
group->getCurrentRPY().at(2));
tf_vec3.setValue(wrench.force.x,wrench.force.y,wrench.force.z);
tf_vec3 = rot_mat6wrt0.transpose() * tf_vec3;
wrench.force.x = tf_vec3.getX();
wrench.force.y = tf_vec3.getY();
wrench.force.z = tf_vec3.getZ();
}
void place(moveit::planning_interface::MoveGroup &group)
{
std::vector<manipulation_msgs::PlaceLocation> loc;
geometry_msgs::PoseStamped p;
p.header.frame_id = "base_footprint";
p.pose.position.x = 0.7;
p.pose.position.y = 0.0;
p.pose.position.z = 0.5;
p.pose.orientation.x = 0;
p.pose.orientation.y = 0;
p.pose.orientation.z = 0;
p.pose.orientation.w = 1;
manipulation_msgs::PlaceLocation g;
g.place_pose = p;
g.approach.direction.vector.z = -1.0;
g.retreat.direction.vector.x = -1.0;
g.retreat.direction.header.frame_id = "base_footprint";
g.approach.direction.header.frame_id = "r_wrist_roll_link";
g.approach.min_distance = 0.1;
g.approach.desired_distance = 0.2;
g.retreat.min_distance = 0.1;
g.retreat.desired_distance = 0.25;
g.post_place_posture.name.resize(1, "r_gripper_joint");
g.post_place_posture.position.resize(1);
g.post_place_posture.position[0] = 1;
loc.push_back(g);
group.setSupportSurfaceName("table");
// add path constraints
moveit_msgs::Constraints constr;
constr.orientation_constraints.resize(1);
moveit_msgs::OrientationConstraint &ocm = constr.orientation_constraints[0];
ocm.link_name = "r_wrist_roll_link";
ocm.header.frame_id = p.header.frame_id;
ocm.orientation.x = 0.0;
ocm.orientation.y = 0.0;
ocm.orientation.z = 0.0;
ocm.orientation.w = 1.0;
ocm.absolute_x_axis_tolerance = 0.2;
ocm.absolute_y_axis_tolerance = 0.2;
ocm.absolute_z_axis_tolerance = M_PI;
ocm.weight = 1.0;
group.setPathConstraints(constr);
group.setPlannerId("RRTConnectkConfigDefault");
group.place("part", loc);
}
moveit_msgs::CollisionObject createCollisionBox(
moveit::planning_interface::MoveGroup& group, std::string object_id,
double size_x, double size_y, double size_z, double pos_x, double pos_y,
double pos_z, double orientation_x, double orientation_y,
double orientation_z, double orientation_w) {
moveit_msgs::CollisionObject collision_object;
collision_object.header.frame_id = group.getPlanningFrame();
/* The id of the object is used to identify it. */
collision_object.id = object_id;
/* Define a box to add to the world. */
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = size_x;
primitive.dimensions[1] = size_y;
primitive.dimensions[2] = size_z;
/* A pose for the box (specified relative to frame_id) */
geometry_msgs::Pose box_pose;
box_pose.orientation.x = orientation_x;
box_pose.orientation.y = orientation_y;
box_pose.orientation.z = orientation_z;
box_pose.orientation.w = orientation_w;
box_pose.position.x = pos_x;
box_pose.position.y = pos_y;
box_pose.position.z = pos_z;
collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(box_pose);
collision_object.operation = collision_object.ADD;
return collision_object;
}
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 pick(moveit::planning_interface::MoveGroup &group)
{
std::vector<manipulation_msgs::Grasp> grasps;
geometry_msgs::PoseStamped p;
p.header.frame_id = "base_footprint";
p.pose.position.x = 0.32;
p.pose.position.y = -0.7;
p.pose.position.z = 0.5;
p.pose.orientation.x = 0;
p.pose.orientation.y = 0;
p.pose.orientation.z = 0;
p.pose.orientation.w = 1;
manipulation_msgs::Grasp g;
g.grasp_pose = p;
g.approach.direction.vector.x = 1.0;
g.approach.direction.header.frame_id = "r_wrist_roll_link";
g.approach.min_distance = 0.2;
g.approach.desired_distance = 0.4;
g.retreat.direction.header.frame_id = "base_footprint";
g.retreat.direction.vector.z = 1.0;
g.retreat.min_distance = 0.1;
g.retreat.desired_distance = 0.25;
g.pre_grasp_posture.name.resize(1, "r_gripper_joint");
g.pre_grasp_posture.position.resize(1);
g.pre_grasp_posture.position[0] = 1;
g.grasp_posture.name.resize(1, "r_gripper_joint");
g.grasp_posture.position.resize(1);
g.grasp_posture.position[0] = 0;
grasps.push_back(g);
group.setSupportSurfaceName("table");
group.pick("part", grasps);
}
void tiltHead(moveit::planning_interface::MoveGroup &head, double value)
{
head.setJointValueTarget("head_tilt_joint", value);
head.move();
}
void HapticsPSM::run_haptic_alg(){
if(check_collison()){
//Step 1:
get_collision_normals_and_points(coll_res.contacts, coll_psm.cur_normal_arr, coll_psm.cur_contact_pnts_arr, coll_psm.insertion_depths);
if (coll_psm.contact_cnts_prev != coll_res.contacts.size()){
ROS_INFO("Number of Contact Points = %i", coll_res.contacts.size());
}
//Step 2:
compute_average_position(coll_psm.cur_contact_pnts_arr, coll_psm.locked_position);
compute_average_normal(coll_psm.cur_normal_arr, coll_psm.cur_normal);
if(coll_psm._first_contact){
ROS_INFO("First contact occured");
tf::Vector3 diff, cur_pos;
get_current_position(cur_pos);
diff = coll_psm.locked_position - cur_pos;
coll_psm.spr_radius = diff.length();
ROS_INFO("SPR Radius Detected to be %f",coll_psm.spr_radius);
coll_psm._first_contact = false;
ROS_INFO("Normal is nx = %f ny = %f nz = %f", coll_psm.cur_normal.getX(),coll_psm.cur_normal.getY(),coll_psm.cur_normal.getZ());
}
//Step 3:
if(has_normal_changed(coll_psm.cur_normal, coll_psm.pre_normal)){
// ROS_INFO("Normal has Changed nx = %f ny = %f nz = %f", coll_psm.cur_normal.getX(),coll_psm.cur_normal.getY(),coll_psm.cur_normal.getZ());
// ROS_INFO("Normal has Changed pnx = %f pny = %f pnz = %f", coll_psm.pre_normal.getX(),coll_psm.pre_normal.getY(),coll_psm.pre_normal.getZ());
}
//Step 5:
compute_total_deflection(coll_psm.def_total);
//Step 6:
compute_deflection_along_normal(coll_psm.cur_normal, coll_psm.def_total, coll_psm.def_along_n);
//Step 7:
compute_deflection_force(spr_haptic_force.wrench,coll_psm.def_along_n);
//Step 8:
compute_force_in_tip_frame(spr_haptic_force.wrench);
//Step 9:
coll_psm.pre_normal = coll_psm.cur_normal;
coll_psm.contact_cnts_prev = coll_res.contacts.size();
if (coll_psm.cur_normal.angle(coll_psm.def_along_n) > coll_psm.epsilon){
ROS_INFO("Angle between normal and deflection force %f", coll_psm.cur_normal.angle(coll_psm.def_along_n));
}
mesh_normal.wrench.force.x = coll_psm.cur_normal.getX();
mesh_normal.wrench.force.y = coll_psm.cur_normal.getY();
mesh_normal.wrench.force.z = coll_psm.cur_normal.getZ();
normal_pub.publish(mesh_normal);
}
else{
if(!coll_psm._first_contact){
ROS_INFO("First contact Released");
coll_psm._first_contact = true;
}
coll_psm.def_along_n.setValue(0,0,0);
coll_psm.cur_normal.setValue(0,0,0);
spr_haptic_force.wrench.force.x=0;
spr_haptic_force.wrench.force.y=0;
spr_haptic_force.wrench.force.z=0;
}
haptic_deflection.x = coll_psm.def_along_n.getX();
haptic_deflection.y = coll_psm.def_along_n.getY();
haptic_deflection.z = coll_psm.def_along_n.getZ();
haptic_deflection_pub.publish(haptic_deflection);
spr_haptic_pub.publish(spr_haptic_force);
pose_pub.publish(group->getCurrentPose());
}
void HapticsPSM::get_current_position(tf::Vector3 &v){
v.setValue(group->getCurrentPose().pose.position.x,
group->getCurrentPose().pose.position.y,
group->getCurrentPose().pose.position.z);
}
void Actuator::executeMotion(const amazon::MoveToGoalConstPtr& goal){
if(goal->moveAction == amazon::MoveToGoal::MOVE_TO_POSE){
ROS_INFO("Moving arm to specified location");
if(moveToPose(goal->movePose, goal->arm))
moveToServer.setSucceeded();
else
moveToServer.setAborted();
}
else if (goal->moveAction == amazon::MoveToGoal::MOVE_TO_PICK){
// Do some error handling
if(goal->shelf.data.size() != 1 || goal->shelf.data.at(0) > 'L' || goal->shelf.data.at(0) < 'A'){
ROS_WARN("Incorrect shelf name passed in");
moveToServer.setAborted();
}
else if(!isInsideShelf(goal->movePose.position, goal->shelf.data.at(0))){
ROS_WARN("Point is not inside specified shelf");
moveToServer.setAborted();
}
else{
ROS_INFO("Moving arm to front of shelf %s", goal->shelf.data.c_str());
geometry_msgs::Pose shelfPose;
shelfPose.position = shelf_positions.at(goal->shelf.data.at(0));
// make orientation horizontal in front of shelf
shelfPose.orientation.w = 0.7071;
shelfPose.orientation.y = 0.7071;
// move to point in front of shelf, if successful, move into shelf
if(moveToPose(shelfPose, goal->arm) && pushGrip(goal->arm, goal->shelf.data.at(0), goal->movePose.position)){// && moveToPose(goal->movePose, goal->arm)){
moveToServer.setSucceeded();
}
else
moveToServer.setAborted();
}
}
else if (goal->moveAction == amazon::MoveToGoal::MOVE_TO_SHELF){
// Do some error handling
if(goal->shelf.data.size() != 1 || goal->shelf.data.at(0) > 'L' || goal->shelf.data.at(0) < 'A'){
ROS_WARN("Incorrect shelf name passed in");
moveToServer.setAborted();
}
else{
ROS_INFO("Moving arm to front of shelf %s", goal->shelf.data.c_str());
geometry_msgs::Pose shelfPose;
shelfPose.position = shelf_positions.at(goal->shelf.data.at(0));
// make orientation horizontal in front of shelf
shelfPose.orientation.w = 0.7071;
shelfPose.orientation.y = 0.7071;
// move to point in front of shelf, if successful, move into shelf
if(moveToPose(shelfPose, goal->arm)){
moveToServer.setSucceeded();
}
else
moveToServer.setAborted();
}
}
else if (goal->moveAction == amazon::MoveToGoal::MOVE_TO_DROP){
// Do some error handling
if(goal->shelf.data.size() != 1 || goal->shelf.data.at(0) > 'L' || goal->shelf.data.at(0) < 'A'){
ROS_WARN("Incorrect shelf name passed in");
moveToServer.setAborted();
}
else if(!isInsideShelf(rightArm.getCurrentPose().pose.position, goal->shelf.data.at(0))){
ROS_WARN("Robot is not inside specified shelf");
moveToServer.setAborted();
}
else{
ROS_INFO("Moving arm to front of shelf %s and then drop location", goal->shelf.data.c_str());
geometry_msgs::Pose shelfPose;
shelfPose.position = shelf_positions.at(goal->shelf.data.at(0));
// make orientation horizontal in front of shelf
shelfPose.orientation.w = 0.7071;
shelfPose.orientation.y = 0.7071;
// move to point in front of shelf, if successful, move into shelf
if(moveToPose(shelfPose, goal->arm) && moveToPose(goal->movePose, goal->arm)){
moveToServer.setSucceeded();
}
else
moveToServer.setAborted();
}
}
else if (goal->moveAction == amazon::MoveToGoal::SCOOP_SHELF){
// Do some error handling
if(goal->shelf.data.size() != 1 || goal->shelf.data.at(0) > 'L' || goal->shelf.data.at(0) < 'A'){
ROS_WARN("Incorrect shelf name passed in");
moveToServer.setAborted();
}
else{
ROS_INFO("Moving arm to front of shelf %s", goal->shelf.data.c_str());
geometry_msgs::Pose shelfPose;
shelfPose.position = shelf_positions.at(goal->shelf.data.at(0));
shelfPose.position.x -= SCOOP_LENGTH;
// make orientation the same as the slope of shelf
shelfPose.orientation.w = 0.4777;
shelfPose.orientation.x = -0.5214;
shelfPose.orientation.y = 0.5214;
shelfPose.orientation.z = -0.4777;
// move to point in front of shelf, if successful, move into shelf
if(moveToPose(shelfPose, goal->arm)){
moveToServer.setSucceeded();
}
else
moveToServer.setAborted();
}
}
}
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
//}
}
//引用传参比较好，不改变值的情况下生明为const安全。
void add_object(const moveit::planning_interface::MoveGroup &group)
{
ros::NodeHandle node_handle;
//添加物体
// Advertise the required topic
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
// Note that this topic may need to be remapped in the launch file
ros::Publisher planning_scene_diff_publisher = node_handle.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
while(planning_scene_diff_publisher.getNumSubscribers() < 1)
{
ros::WallDuration sleep_t(0.5);
sleep_t.sleep();
}
// Define the attached object message
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
// We will use this message to add or
// subtract the object from the world
// and to attach the object to the robot
moveit_msgs::AttachedCollisionObject attached_object;
attached_object.link_name = "lLink7";
/* The header must contain a valid TF frame*/
attached_object.object.header.frame_id = group.getPlanningFrame();
/* The id of the object */
attached_object.object.id = "box1";
/* A default left pose */
geometry_msgs::Pose pose1;
pose1.orientation.w = 1.0;
pose1.position.x=0.4;
pose1.position.y=0.64;
pose1.position.z=0.6;
/* Define a left box to be attached */
shape_msgs::SolidPrimitive primitive1;
primitive1.type = primitive1.BOX;
primitive1.dimensions.resize(3);
primitive1.dimensions[0] = 0.03;
primitive1.dimensions[1] = 0.03;
primitive1.dimensions[2] = 0.2;
/* A default right pose */
geometry_msgs::Pose pose2;
pose2.orientation.w = 1.0;
pose2.position.x=-0.4;
pose2.position.y=0.7;
pose2.position.z=0.4;
/* Define a right box to be attached */
shape_msgs::SolidPrimitive primitive2;
primitive2.type = primitive2.BOX;
primitive2.dimensions.resize(3);
primitive2.dimensions[0] = 0.3;
primitive2.dimensions[1] = 0.3;
primitive2.dimensions[2] = 0.4;
//容器使用push_back进行添加元素
attached_object.object.primitives.push_back(primitive1);
attached_object.object.primitive_poses.push_back(pose1);
attached_object.object.primitives.push_back(primitive2);
attached_object.object.primitive_poses.push_back(pose2);
// Note that attaching an object to the robot requires
// the corresponding operation to be specified as an ADD operation
attached_object.object.operation = attached_object.object.ADD;
// Add an object into the environment
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
// Add the object into the environment by adding it to
// the set of collision objects in the "world" part of the
// planning scene. Note that we are using only the "object"
// field of the attached_object message here.
ROS_INFO("Adding the object into the world ");
moveit_msgs::PlanningScene planning_scene;
planning_scene.world.collision_objects.push_back(attached_object.object);
planning_scene.is_diff = true;
planning_scene_diff_publisher.publish(planning_scene);
sleep(2);
}