int ShapeGraspPlanner::createGrasp(const geometry_msgs::PoseStamped& pose, double gripper_opening, double gripper_pitch, double x_offset, double z_offset, double quality) { moveit_msgs::Grasp grasp; grasp.grasp_pose = pose; // defaults grasp.pre_grasp_posture = makeGraspPosture(gripper_opening); grasp.grasp_posture = makeGraspPosture(0.0); grasp.pre_grasp_approach = makeGripperTranslation(approach_frame_, approach_min_translation_, approach_desired_translation_); grasp.post_grasp_retreat = makeGripperTranslation(retreat_frame_, retreat_min_translation_, retreat_desired_translation_, -1.0); // retreat is in negative x direction // initial pose Eigen::Affine3d p = Eigen::Translation3d(pose.pose.position.x, pose.pose.position.y, pose.pose.position.z) * Eigen::Quaterniond(pose.pose.orientation.w, pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z); // translate by x_offset, 0, z_offset p = p * Eigen::Translation3d(x_offset, 0, z_offset); // rotate by 0, pitch, 0 p = p * quaternionFromEuler(0.0, gripper_pitch, 0.0); // apply grasp point -> planning frame offset p = p * Eigen::Translation3d(-tool_offset_, 0, 0); grasp.grasp_pose.pose.position.x = p.translation().x(); grasp.grasp_pose.pose.position.y = p.translation().y(); grasp.grasp_pose.pose.position.z = p.translation().z(); Eigen::Quaterniond q = (Eigen::Quaterniond)p.linear(); grasp.grasp_pose.pose.orientation.x = q.x(); grasp.grasp_pose.pose.orientation.y = q.y(); grasp.grasp_pose.pose.orientation.z = q.z(); grasp.grasp_pose.pose.orientation.w = q.w(); grasp.grasp_quality = quality; grasps_.push_back(grasp); return 1; }
int ShapeGraspPlanner::plan(const grasping_msgs::Object& object, std::vector<moveit_msgs::Grasp>& grasps) { ROS_INFO("shape grasp planning starting..."); // Need a shape primitive if (object.primitives.size() == 0) { // Shape grasp planner can only plan for objects // with SolidPrimitive bounding boxes return -1; } if (object.primitive_poses.size() != object.primitives.size()) { // Invalid object return -1; } // Clear out internal vector grasps_.clear(); // Setup Pose geometry_msgs::PoseStamped grasp_pose; grasp_pose.header = object.header; grasp_pose.pose = object.primitive_poses[0]; // Setup object orientation Eigen::Quaterniond q(object.primitive_poses[0].orientation.w, object.primitive_poses[0].orientation.x, object.primitive_poses[0].orientation.y, object.primitive_poses[0].orientation.z); // Setup object dimensions double x, y, z; if (object.primitives[0].type == SolidPrimitive::BOX) { x = object.primitives[0].dimensions[SolidPrimitive::BOX_X]; y = object.primitives[0].dimensions[SolidPrimitive::BOX_Y]; z = object.primitives[0].dimensions[SolidPrimitive::BOX_Z]; } else if (object.primitives[0].type == SolidPrimitive::CYLINDER) { x = y = 2.0 * object.primitives[0].dimensions[SolidPrimitive::CYLINDER_RADIUS]; z = object.primitives[0].dimensions[SolidPrimitive::CYLINDER_HEIGHT]; } // Generate grasps for (int i = 0; i < 4; ++i) { grasp_pose.pose.orientation.x = q.x(); grasp_pose.pose.orientation.y = q.y(); grasp_pose.pose.orientation.z = q.z(); grasp_pose.pose.orientation.w = q.w(); if (i < 2) { createGraspSeries(grasp_pose, x, y, z); } else { // Only two sets of unqiue vertical grasps createGraspSeries(grasp_pose, x, y, z, false); } // Next iteration, rotate 90 degrees about Z axis q = q * quaternionFromEuler(-1.57, 0.0, 0.0); std::swap(x, y); } ROS_INFO("shape grasp planning done."); grasps = grasps_; return grasps.size(); // num of grasps }
void Game::initialize(void) { timer_init(&_timer); _frame_count = 0; _render = Render::create(); _render->initialize(app_get_window()); _world.add_system(new RenderSystem, kRenderComponent); _world.add_system(new LightSystem, kLightComponent); RenderData::_render = _render; LightData::_render = _render; //srand((uint32_t)_timer.start_time); srand(42); // Texture loaders _resource_manager.add_handlers("jpg", Render::load_texture, Render::unload_texture, _render); _resource_manager.add_handlers("dds", Render::load_texture, Render::unload_texture, _render); _resource_manager.add_handlers("png", Render::load_texture, Render::unload_texture, _render); _resource_manager.add_handlers("tga", Render::load_texture, Render::unload_texture, _render); // File loaders _resource_manager.add_handlers("mesh", Render::load_mesh, Render::unload_mesh, _render); _resource_manager.add_handlers("obj", Render::load_mesh, Render::unload_mesh, _render); // Create some materials Material grass_material = { _resource_manager.get_resource("assets/grass.dds"), _resource_manager.get_resource("assets/grass_nrm.png"), {0}, {0.0f, 0.0f, 0.0f}, 0.0f, 0.0f }; Material materials[3] = { { _resource_manager.get_resource("assets/metal.dds"), _resource_manager.get_resource("assets/metal_nrm.png"), _resource_manager.get_resource("assets/metal.dds"), {0.0f, 0.0f, 0.0f}, 200.0f, 0.8f }, { _resource_manager.get_resource("assets/brick.dds"), _resource_manager.get_resource("assets/brick_nrm.png"), 0, {1.0f, 1.0f, 1.0f}, 4.0f, 0.1f }, { _resource_manager.get_resource("assets/wood.dds"), _resource_manager.get_resource("assets/wood_nrm.png"), 0, {1.0f, 1.0f, 1.0f}, 8.0f, 0.3f } }; // Ground const VtxPosNormTex ground_vertices[] = { /* Top */ { {-0.5f, 0.0f, -0.5f}, { 0.0f, 1.0f, 0.0f}, {0.0f, 10.0f} }, { { 0.5f, 0.0f, -0.5f}, { 0.0f, 1.0f, 0.0f}, {10.0f, 10.0f} }, { { 0.5f, 0.0f, 0.5f}, { 0.0f, 1.0f, 0.0f}, {10.0f, 0.0f} }, { {-0.5f, 0.0f, 0.5f}, { 0.0f, 1.0f, 0.0f}, {0.0f, 0.0f} }, /* Bottom */ { {-0.5f, 0.0f, -0.5f}, { 0.0f, -1.0f, 0.0f}, {0.0f, 10.0f} }, { { 0.5f, 0.0f, -0.5f}, { 0.0f, -1.0f, 0.0f}, {10.0f, 10.0f} }, { { 0.5f, 0.0f, 0.5f}, { 0.0f, -1.0f, 0.0f}, {10.0f, 0.0f} }, { {-0.5f, 0.0f, 0.5f}, { 0.0f, -1.0f, 0.0f}, {0.0f, 0.0f} }, }; const unsigned short ground_indices[] = { 3,1,0, 2,1,3, 3,0,1, 2,3,1, }; assert(ground_indices && ground_vertices); Transform transform = TransformZero(); transform.scale = 1.0f; transform.position.y = -100000.0f; transform.position.y = 0.0f; RenderData render_data = {0}; //render_data.mesh = _render->create_mesh(ARRAYSIZE(ground_vertices), kVtxPosNormTex, ARRAYSIZE(ground_indices), sizeof(ground_indices[0]), ground_vertices, ground_indices); //render_data.mesh = _render->sphere_mesh(); std::map<float3, VtxPosNormTex> m; std::vector<float3> verts; std::vector<VtxPosNormTex> terrain_verts; std::vector<uint32_t> terrain_indices; verts.reserve(1000000); terrain_verts.reserve(1000000); terrain_indices.reserve(1000000); const float size = 50.0f; float3 min = { -size, -size, -size }; float3 max = { size, size, size }; //min = float3addScalar(&min,) timer_reset(&_timer); generate_terrain_points(terrain_func, min, max, 1.0f, verts); debug_output("Time: %f\tNum raw Vertices: %d\n", timer_delta_time(&_timer), verts.size()); timer_reset(&_timer); smooth_terrain(verts, terrain_verts, terrain_indices); //generate_terrain(terrain_func, terrain_verts, terrain_indices); debug_output("time: %f, Num Terrain Vertices: %d\n", timer_delta_time(&_timer), terrain_verts.size()); debug_output("Num Terrain indices: %d\n", terrain_indices.size()); timer_reset(&_timer); render_data.mesh = _render->create_mesh((uint32_t)terrain_verts.size(), kVtxPosNormTex, (uint32_t)terrain_indices.size(), sizeof(uint32_t), terrain_verts.data(), terrain_indices.data()); render_data.material = grass_material; EntityID id = _world.create_entity(); _world.entity(id)->set_transform(transform) ->add_component(RenderComponent(render_data)); id = _world.create_entity(); Material sky_material = { _resource_manager.get_resource("assets/sky.png"), _resource_manager.get_resource("assets/default_norm.png"), {0}, {0.0f, 0.0f, 0.0f}, 0.0f, 0.0f }; render_data.material = sky_material; render_data.mesh = _render->sphere_mesh(); transform = TransformZero(); transform.scale = -100.0f; transform.scale = 1000.0f; _world.entity(id)->set_transform(transform) ->add_component(RenderComponent(render_data)); for(int ii=0; ii<32;++ii) { transform = TransformZero(); transform.scale = _rand_float(0.5f, 5.0f); transform.position.x = _rand_float(-50.0f, 50.0f); transform.position.y = _rand_float(3.0f, 7.5f); transform.position.z = _rand_float(-50.0f, 50.0f); float3 axis = { _rand_float(-3.0f, 3.0f), _rand_float(-3.0f, 3.0f), _rand_float(-3.0f, 3.0f) }; transform.orientation = quaternionFromAxisAngle(&axis, _rand_float(0.0f, kPi)); switch(rand()%2) { case 0: render_data.mesh = _render->sphere_mesh(); break; case 1: render_data.mesh = _render->cube_mesh(); break; } int material = rand()%3; render_data.material = materials[material]; id = _world.create_entity(); _world.entity(id)->set_transform(transform) ->add_component(RenderComponent(render_data)); } Material house_material = { _resource_manager.get_resource("assets/house_diffuse.tga"), _resource_manager.get_resource("assets/house_normal.tga"), _resource_manager.get_resource("assets/house_spec.tga"), {0.0f, 0.0f, 0.0f}, 1.5f, 0.06f }; transform = TransformZero(); transform.scale = 0.015f; transform.orientation = quaternionFromEuler(0.0f, DegToRad(-90.0f), 0.0f); render_data.mesh = _resource_manager.get_resource("assets/house_obj.obj"); render_data.material = house_material; id = _world.create_entity(); _world.entity(id)->set_transform(transform) ->add_component(RenderComponent(render_data)); // Add a "sun" Light light; light.pos.x = 0.0f; light.pos.y = 10.0f; light.pos.z = 0.0f; light.size = 250.0f; light.dir.x = 0.0f; light.dir.y = -1.0f; light.dir.z = 0.0f; light.color.x = 1.0f; light.color.y = 1.0f; light.color.z = 1.0f; light.inner_cos = 0.1f; light.type = kDirectionalLight; transform = TransformZero(); transform.orientation = quaternionFromEuler(DegToRad(90.0f), DegToRad(45.0f), 0.0f); transform.position = light.pos; _sun_id = _world.create_entity(); _world.entity(_sun_id)->set_transform(transform) ->add_component(LightComponent(light)); transform = TransformZero(); for(int ii=1;ii<MAX_LIGHTS;++ii) { light.pos.x = _rand_float(-50.0f, 50.0f); light.pos.y = _rand_float(1.0f, 4.0f); light.pos.z = _rand_float(-50.0f, 50.0f); light.size = 3.0f; light.color.x = _rand_float(0.0f, 1.0f); light.color.y = _rand_float(0.0f, 1.0f); light.color.z = _rand_float(0.0f, 1.0f); light.type = kPointLight; transform.position = light.pos; id = _world.create_entity(); _world.entity(id)->set_transform(transform) ->add_component(LightComponent(light)); } }