[ERROR] : Unable to transform from frame '' to frame 'base_link'. Returning identity.Computed path is not valid. Invalid states at index locations: [ 29 47 52 63 78 ] out of 93. Explanations follow in command line. Contacts are published on /move_group/display_contactsFound a contact between 'link1' (type 'Robot link') and 'gripper_link1' (type 'Robot link'), which constitutes a collision. Contact information is not stored.[ INFO] [1648102511.741988280]: Collision checking is considered complete (collision was found and 0 contacts are stored)Completed listing of explanations for invalid states.报错就是以上这些重复的报错。现在问题就是,抓不起来,根本就没有过去抓取的位置。代码如下:这是主函数。void Gripper::gripper_object(){ ros::NodeHandle nh; ros::AsyncSpinner spinner(1); spinner.start(); moveit::planning_interface::MoveGroupInterface armgroup("arm"); armgroup.setPlanningTime(45.0); ros::WallDuration(1.0).sleep(); log(Info," move is ok! "); sleep(4); std::vector<moveit_msgs::Grasp> grasps; grasps.resize(5); grasps[0].grasp_pose.header.frame_id = "base_link"; grasps[0].grasp_pose.pose.orientation.w = 1.0; grasps[0].grasp_pose.pose.orientation.x = 0.0; grasps[0].grasp_pose.pose.orientation.y = 0.0; grasps[0].grasp_pose.pose.orientation.z = 0.0; grasps[0].grasp_pose.pose.position.x = 2.5; grasps[0].grasp_pose.pose.position.y = 0.96; grasps[0].grasp_pose.pose.position.z =1.2 ; grasps[0].pre_grasp_approach.direction.header.frame_id = "base_link"; /* Direction is set as positive x axis */ grasps[0].pre_grasp_approach.direction.vector.y = 1.0; grasps[0].pre_grasp_approach.min_distance = 0.095; grasps[0].pre_grasp_approach.desired_distance = 0.115; grasps[0].post_grasp_retreat.direction.header.frame_id = "base_link"; /* Direction is set as positive z axis */ grasps[0].post_grasp_retreat.direction.vector.z = 1.0; grasps[0].post_grasp_retreat.min_distance = 0.1; grasps[0].post_grasp_retreat.desired_distance = 0.25; openGripper(grasps[0].pre_grasp_posture); closedGripper(grasps[0].grasp_posture); armgroup.setSupportSurfaceName("table"); armgroup.pick("box",grasps); ros::WallDuration(1.0).sleep(); ros::waitForShutdown();}这是打开grippervoid Gripper::openGripper(trajectory_msgs::JointTrajectory& posture){ posture.joint_names.resize(2); posture.joint_names[0] = "gripper_joint1"; posture.joint_names[1] = "gripper_joint2"; posture.points.resize(1); posture.points[0].positions.resize(2); posture.points[0].positions[0] = -0.04; posture.points[0].positions[1] = 0.04; posture.points[0].time_from_start = ros::Duration(0.5);}这是关闭grippervoid Gripper::closedGripper(trajectory_msgs::JointTrajectory& posture){ posture.joint_names.resize(2); posture.joint_names[0] = "gripper_joint1"; posture.joint_names[1] = "gripper_joint2"; posture.points.resize(1); posture.points[0].positions.resize(2); posture.points[0].positions[0] = 0.12; posture.points[0].positions[1] = -0.09; posture.points[0].time_from_start = ros::Duration(0.5);}这是添加物体的实现。void Gripper::on_add_object_Button_clicked(){ double table_size[3] = { 3.0, 6.0, 0.25 }; double box1_size[3] = { 0.6, 0.3, 0.3 }; ros::NodeHandle nh; plan_scene_table_pub =nh.advertise<moveit_msgs::PlanningScene>("/planning_scene",1); plan_scene_box_pub =nh.advertise<moveit_msgs::PlanningScene>("/planning_scene",1); sleep(2); while(plan_scene_table_pub.getNumSubscribers() < 1) { sleep(1); } while(plan_scene_box_pub.getNumSubscribers() < 1) { sleep(1); } moveit_msgs::CollisionObject ob; ob.header.frame_id = "base_link"; ob.id="table"; moveit_msgs::ObjectColor table_color; table_color.id="table"; table_color.color.a=1; table_color.color.b=1; table_color.color.g=1; table_color.color.r=1; shape_msgs::SolidPrimitive primitive; primitive.type = primitive.BOX; primitive.dimensions.resize(3); primitive.dimensions[0] = table_size[0]; primitive.dimensions[1] = table_size[1]; primitive.dimensions[2] = table_size[2]; geometry_msgs::Pose pose; pose.orientation.w = 1; pose.position.x=3; pose.position.z=1; ob.primitives.push_back(primitive); ob.primitive_poses.push_back(pose); ob.operation = ob.ADD; moveit_msgs::PlanningScene planning_scene; planning_scene.object_colors.push_back(table_color); planning_scene.world.collision_objects.push_back(ob); planning_scene.is_diff = true; // box ob_gripper_box.header.frame_id = "base_link"; ob_gripper_box.id="box"; primitive_box.type = primitive_box.BOX; primitive_box.dimensions.resize(3); primitive_box.dimensions[0] = box1_size[0]; primitive_box.dimensions[1] = box1_size[1]; primitive_box.dimensions[2] = box1_size[2]; pose_box.orientation.w = 1; pose_box.position.x=2.5; pose_box.position.y=1; pose_box.position.z= pose.position.z +(box1_size[2] / 2) + (table_size[2]/2); //1.275 ob_gripper_box.primitives.push_back(primitive_box); ob_gripper_box.primitive_poses.push_back(pose_box); ob_gripper_box.operation = ob_gripper_box.ADD; planning_scene_box.world.collision_objects.push_back(ob_gripper_box); planning_scene_box.is_diff = true; sleep(2); plan_scene_table_pub.publish(planning_scene); sleep(1); plan_scene_box_pub.publish(planning_scene_box); log(Info," Add object is ok ! ");}
展示全文
第三方账号登入
QQ 微博 微信