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));
}
}
