Skip to content

Commit

Permalink
fixing to work with eigen and new search energy factory
Browse files Browse the repository at this point in the history
  • Loading branch information
jvarley committed Feb 12, 2017
1 parent 35d6346 commit 243bcde
Show file tree
Hide file tree
Showing 4 changed files with 22 additions and 64 deletions.
1 change: 0 additions & 1 deletion CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -57,7 +57,6 @@ find_package(Qt4 COMPONENTS QtCore REQUIRED)
Robot.msg
Grasp.msg
Planner.msg
SearchEnergy.msg
SearchSpace.msg
SearchContact.msg
Energy.msg
Expand Down
6 changes: 3 additions & 3 deletions action/PlanGrasps.action
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
int32 graspable_body_id

Planner planner
SearchEnergy search_energy
string search_energy
SearchSpace search_space
SearchContact search_contact

Expand All @@ -11,8 +11,8 @@ int32 max_steps
# Result
Grasp[] grasps
float64[] energies
SearchEnergy search_energy
string search_energy
---
# Feedback
int32 current_step
int32 current_num_grasps
int32 current_num_grasps
7 changes: 0 additions & 7 deletions msg/SearchEnergy.msg

This file was deleted.

72 changes: 19 additions & 53 deletions src/graspit_interface.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -123,10 +123,10 @@ bool GraspitInterface::getRobotCB(graspit_interface::GetRobot::Request &request,
pose.position.x = t.translation().x() / 1000.0;
pose.position.y = t.translation().y() / 1000.0;;
pose.position.z = t.translation().z() / 1000.0;;
pose.orientation.w = t.rotation().w;
pose.orientation.x = t.rotation().x;
pose.orientation.y = t.rotation().y;
pose.orientation.z = t.rotation().z;
pose.orientation.w = t.rotation().w();
pose.orientation.x = t.rotation().x();
pose.orientation.y = t.rotation().y();
pose.orientation.z = t.rotation().z();

response.robot.pose = pose;

Expand Down Expand Up @@ -159,10 +159,10 @@ bool GraspitInterface::getGraspableBodyCB(graspit_interface::GetGraspableBody::R
pose.position.x = t.translation().x() / 1000.0;
pose.position.y = t.translation().y() / 1000.0;;
pose.position.z = t.translation().z() / 1000.0;;
pose.orientation.w = t.rotation().w;
pose.orientation.x = t.rotation().x;
pose.orientation.y = t.rotation().y;
pose.orientation.z = t.rotation().z;
pose.orientation.w = t.rotation().w();
pose.orientation.x = t.rotation().x();
pose.orientation.y = t.rotation().y();
pose.orientation.z = t.rotation().z();

response.graspable_body.pose = pose;
return true;
Expand All @@ -185,10 +185,10 @@ bool GraspitInterface::getBodyCB(graspit_interface::GetBody::Request &request,
pose.position.x = t.translation().x() / 1000.0;
pose.position.y = t.translation().y() / 1000.0;;
pose.position.z = t.translation().z() / 1000.0;;
pose.orientation.w = t.rotation().w;
pose.orientation.x = t.rotation().x;
pose.orientation.y = t.rotation().y;
pose.orientation.z = t.rotation().z;
pose.orientation.w = t.rotation().w();
pose.orientation.x = t.rotation().x();
pose.orientation.y = t.rotation().y();
pose.orientation.z = t.rotation().z();

response.body.pose = pose;

Expand Down Expand Up @@ -704,43 +704,9 @@ void GraspitInterface::runPlannerInMainThread()
}
}

switch(goal.search_energy.type) {
case graspit_interface::SearchEnergy::ENERGY_CONTACT_QUALITY :
{
mPlanner->setEnergyType(ENERGY_CONTACT_QUALITY);
ROS_INFO("Using graspit_interface::SearchEnergy::ENERGY_CONTACT_QUALITY ");
break;
}
case graspit_interface::SearchEnergy::ENERGY_POTENTIAL_QUALITY :
{
mPlanner->setEnergyType(ENERGY_POTENTIAL_QUALITY);
ROS_INFO("Using graspit_interface::SearchEnergy::ENERGY_POTENTIAL_QUALITY ");
break;
}
case graspit_interface::SearchEnergy::ENERGY_CONTACT :
{
mPlanner->setEnergyType(ENERGY_CONTACT);
ROS_INFO("Using graspit_interface::SearchEnergy::ENERGY_CONTACT ");
break;
}
case graspit_interface::SearchEnergy::ENERGY_AUTOGRASP_QUALITY :
{
mPlanner->setEnergyType(ENERGY_AUTOGRASP_QUALITY);
ROS_INFO("Using graspit_interface::SearchEnergy::ENERGY_AUTOGRASP_QUALITY ");
break;
}
case graspit_interface::SearchEnergy::ENERGY_GUIDED_AUTOGRASP :
{
mPlanner->setEnergyType(ENERGY_GUIDED_AUTOGRASP);
ROS_INFO("Using graspit_interface::SearchEnergy::ENERGY_GUIDED_AUTOGRASP ");
break;
}
default:
{
ROS_INFO("Invalid Search Energy Type");
//return;
}
}

mPlanner->setEnergyType(goal.search_energy);


switch(goal.search_contact.type) {
case graspit_interface::SearchContact::CONTACT_PRESET :
Expand Down Expand Up @@ -810,10 +776,10 @@ void GraspitInterface::runPlannerInMainThread()
pose.position.x = t.translation().x() / 1000.0;
pose.position.y = t.translation().y() / 1000.0;;
pose.position.z = t.translation().z() / 1000.0;;
pose.orientation.w = t.rotation().w;
pose.orientation.x = t.rotation().x;
pose.orientation.y = t.rotation().y;
pose.orientation.z = t.rotation().z;
pose.orientation.w = t.rotation().w();
pose.orientation.x = t.rotation().x();
pose.orientation.y = t.rotation().y();
pose.orientation.z = t.rotation().z();

graspit_interface::Grasp g;
g.graspable_body_id = goal.graspable_body_id;
Expand Down

0 comments on commit 243bcde

Please sign in to comment.