Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[4197] pkg/trunk
|
From: <is...@us...> - 2008-09-11 07:22:58
|
Revision: 4197
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4197&view=rev
Author: isucan
Date: 2008-09-11 07:23:09 +0000 (Thu, 11 Sep 2008)
Log Message:
-----------
code for demo....
Modified Paths:
--------------
pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
pkg/trunk/world_models/fake_world_3d_map/src/fake_world_3d_map.cpp
Modified: pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2008-09-11 07:19:55 UTC (rev 4196)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2008-09-11 07:23:09 UTC (rev 4197)
@@ -44,7 +44,7 @@
#include <robot_msgs/DisplayKinematicPath.h>
#include <robot_srvs/ValidateKinematicPath.h>
-#include <pr2_controllers/SetJointPathCmd.h>
+#include <pr2_controllers/SetJointTarget.h>
#include <cassert>
@@ -86,11 +86,17 @@
req.goal_state.set_vals_size(3);
for (unsigned int i = 0 ; i < req.goal_state.vals_size ; ++i)
req.goal_state.vals[i] = m_basePos[i];
-
+ /*
req.goal_state.vals[0] += 7.5;
req.goal_state.vals[1] += 0.5;
req.goal_state.vals[2] = -M_PI/2.0;
+ */
+ req.goal_state.vals[0] += 5.5;
+ req.goal_state.vals[1] += 0.5;
+ req.goal_state.vals[2] = -M_PI/2.0;
+
+
req.allowed_time = 15.0;
req.params.volumeMin.x = -10.0 + m_basePos[0]; req.params.volumeMin.y = -10.0 + m_basePos[1]; req.params.volumeMin.z = 0.0;
@@ -139,10 +145,10 @@
initialState(req.start_state);
- req.goal_state.set_vals_size(7);
+ req.goal_state.set_vals_size(4);
for (unsigned int i = 0 ; i < req.goal_state.vals_size ; ++i)
req.goal_state.vals[i] = 0.0;
- req.goal_state.vals[0] = -1.0;
+ req.goal_state.vals[0] = -1.3;
req.allowed_time = 30.0;
@@ -185,21 +191,6 @@
performCall(req);
}
- void performCall(robot_srvs::KinematicPlanState::request &req)
- {
- robot_srvs::KinematicPlanState::response res;
-
- if (ros::service::call("plan_kinematic_path_state", req, res))
- {
- printPath(res.path, res.distance);
- sendDisplay(res.path, req.params.model_id);
- verifyPath(req.start_state, req.constraints, res.path, req.params.model_id);
- sendCommand(res.path, req.params.model_id);
- }
- else
- fprintf(stderr, "Service 'plan_kinematic_path_state' failed\n");
- }
-
void runTestLeftEEf(void)
{
robot_srvs::KinematicPlanLinkPosition::request req;
@@ -208,7 +199,7 @@
req.params.distance_metric = "L2Square";
req.params.planner_id = "RRT";
req.interpolate = 1;
- req.times = 4;
+ req.times = 3;
initialState(req.start_state);
@@ -235,7 +226,7 @@
req.constraints.pose[0].pose.position.z = 0.74;
req.constraints.pose[0].position_distance = 0.01;
*/
- req.allowed_time = 0.1;
+ req.allowed_time = 0.3;
req.params.volumeMin.x = -5.0 + m_basePos[0]; req.params.volumeMin.y = -5.0 + m_basePos[1]; req.params.volumeMin.z = 0.0;
req.params.volumeMax.x = 5.0 + m_basePos[0]; req.params.volumeMax.y = 5.0 + m_basePos[1]; req.params.volumeMax.z = 0.0;
@@ -258,10 +249,25 @@
fprintf(stderr, "Service 'plan_kinematic_path_position' failed\n");
}
+ void performCall(robot_srvs::KinematicPlanState::request &req)
+ {
+ robot_srvs::KinematicPlanState::response res;
+
+ if (ros::service::call("plan_kinematic_path_state", req, res))
+ {
+ printPath(res.path, res.distance);
+ sendDisplay(res.path, req.params.model_id);
+ verifyPath(req.start_state, req.constraints, res.path, req.params.model_id);
+ sendCommand(res.path, req.params.model_id);
+ }
+ else
+ fprintf(stderr, "Service 'plan_kinematic_path_state' failed\n");
+ }
+
void printPath(robot_msgs::KinematicPath &path, const double distance)
{
unsigned int nstates = path.get_states_size();
- printf("Obtained ssolution path with %u states, distance to goal = %f\n", nstates, distance);
+ printf("Obtained solution path with %u states, distance to goal = %f\n", nstates, distance);
for (unsigned int i = 0 ; i < nstates ; ++i)
{
@@ -304,12 +310,13 @@
void sendCommand(robot_msgs::KinematicPath &path, const std::string &model)
{
// create the service request
-
+ // return;
+
const double margin_fraction = 0.1;
planning_models::KinematicModel::StateParams *state = m_kmodel->newStateParams();
- pr2_controllers::SetJointPathCmd::request req;
- req.set_path_size(path.get_states_size());
+ pr2_controllers::SetJointTarget::request req;
+ req.set_positions_size(path.get_states_size());
int groupID = m_kmodel->getGroupID(model);
@@ -327,20 +334,20 @@
state->setParams(path.states[i].vals, groupID);
- req.path[i].set_names_size(joints.size());
- req.path[i].set_positions_size(joints.size());
- req.path[i].set_margins_size(joints.size());
- req.path[i].timeout = 1.0;
+ req.positions[i].set_names_size(joints.size());
+ req.positions[i].set_positions_size(joints.size());
+ req.positions[i].set_margins_size(joints.size());
+ req.positions[i].timeout = 10.0;
for (unsigned int j = 0 ; j < joints.size() ; ++j)
{
- req.path[i].names[j] = joints[j];
+ req.positions[i].names[j] = joints[j];
- state->copyParams(&(req.path[i].positions[j]), joints[j]);
+ state->copyParams(&(req.positions[i].positions[j]), joints[j]);
int pos = state->getPos(joints[j]);
assert(pos >= 0);
- req.path[i].margins[j] = margin_fraction * (m_kmodel->stateBounds[2 * pos + 1] - m_kmodel->stateBounds[2 * pos]);
+ req.positions[i].margins[j] = 0.25; // margin_fraction * (m_kmodel->stateBounds[2 * pos + 1] - m_kmodel->stateBounds[2 * pos]);
}
}
@@ -348,9 +355,9 @@
// perform the service call
- pr2_controllers::SetJointPathCmd::response res;
+ pr2_controllers::SetJointTarget::response res;
- if (ros::service::call("left_arm_controller/set_target", req, res))
+ if (ros::service::call("/left_arm_controller/set_target", req, res))
{
}
Modified: pkg/trunk/world_models/fake_world_3d_map/src/fake_world_3d_map.cpp
===================================================================
--- pkg/trunk/world_models/fake_world_3d_map/src/fake_world_3d_map.cpp 2008-09-11 07:19:55 UTC (rev 4196)
+++ pkg/trunk/world_models/fake_world_3d_map/src/fake_world_3d_map.cpp 2008-09-11 07:23:09 UTC (rev 4197)
@@ -108,6 +108,9 @@
case 1:
oneObstacle();
break;
+ case 2:
+ verticalObstacle();
+ break;
case 3:
planeX();
break;
@@ -130,6 +133,33 @@
m_toPublish.pts[0].z = 0.75;
}
+ void verticalObstacle(void)
+ {
+ const int N = 30;
+ const int M = 3;
+ const int L = 3;
+
+ m_toPublish.set_pts_size(N * M * L);
+
+ for (int k = 0 ; k < L ; ++k)
+ {
+ double x = 0.30 + 0.02 * k;
+
+ for (int j = 0 ; j < M ; ++j)
+ {
+ double y = -0.30 - 0.02 * j;
+
+ for (int i = 0 ; i < N ; ++i)
+ {
+ double z = 0.1 + i * 0.05;
+ m_toPublish.pts[i + j*N + k*N*M].x = x;
+ m_toPublish.pts[i + j*N + k*N*M].y = y;
+ m_toPublish.pts[i + j*N + k*N*M].z = z;
+ }
+ }
+ }
+ }
+
void planeX(void)
{
// for (int i = 0 ; i < 20 ; ++i)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
