Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[9098] pkg/trunk
|
From: <sac...@us...> - 2009-01-09 01:27:00
|
Revision: 9098
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9098&view=rev
Author: sachinchitta
Date: 2009-01-09 01:26:53 +0000 (Fri, 09 Jan 2009)
Log Message:
-----------
arm joint space trajectory bug fixes
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_service.cpp
pkg/trunk/robot_descriptions/arm_cart/run_trajectory_controller.launch
pkg/trunk/robot_descriptions/arm_cart/trajectory_controllers.xml
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt 2009-01-09 01:06:35 UTC (rev 9097)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt 2009-01-09 01:26:53 UTC (rev 9098)
@@ -40,3 +40,7 @@
rospack_add_executable(testTraj test/test_arm_trajectory_controller.cpp)
target_link_libraries(testTraj pr2_mechanism_controllers)
+
+rospack_add_executable(testTrajServ test/test_arm_trajectory_service.cpp)
+target_link_libraries(testTrajServ pr2_mechanism_controllers)
+
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp 2009-01-09 01:06:35 UTC (rev 9097)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp 2009-01-09 01:26:53 UTC (rev 9098)
@@ -419,13 +419,31 @@
bool ArmTrajectoryControllerNode::queryJointTrajSrv(pr2_mechanism_controllers::TrajectoryQuery::request &req,
pr2_mechanism_controllers::TrajectoryQuery::response &resp)
{
+ resp.set_jointnames_size(c_->dimension_);
+ for(int i=0; i < c_->dimension_; i++)
+ {
+ resp.jointnames[i] = c_->joint_pd_controllers_[i]->getJointName();
+
+ }
+
+ if(req.trajectoryid == 0)
+ {
+ resp.trajectorytime = 0;
+ if(joint_trajectory_vector_.size() == 0)
+ resp.done = 1;
+ else
+ resp.done = 0;
+ return true;
+ }
+
std::map<int, int>::const_iterator it = joint_trajectory_status_.find((int)req.trajectoryid);
if(it == joint_trajectory_status_.end())
return false;
+ else
+ resp.done = it->second;
+
- resp.done = it->second;
-
- if(current_trajectory_id_ == (int)req.trajectoryid)
+if(current_trajectory_id_ == (int)req.trajectoryid)
{
if((int) resp.done == 1)
resp.trajectorytime = c_->trajectory_end_time_ - c_->trajectory_start_time_;
@@ -440,12 +458,8 @@
resp.trajectorytime = it_time->second;
}
- resp.set_jointnames_size(c_->dimension_);
- for(int i=0; i < c_->dimension_; i++)
- {
- resp.jointnames[i] = c_->joint_pd_controllers_[i]->getJointName();
-
- }
+
+
return true;
}
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_service.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_service.cpp 2009-01-09 01:06:35 UTC (rev 9097)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_service.cpp 2009-01-09 01:26:53 UTC (rev 9098)
@@ -98,21 +98,29 @@
req.traj.points[2].time = 0.0;
- if (ros::service::call("set_arm_traj_srv", req, res))
+ if (ros::service::call("TrajectoryStart", req, res))
{
ROS_INFO("Done");
}
- req_q.trajectoryid = 1;
+ req_q.trajectoryid = atoi(argv[1]);
- if(ros::service::call("query_arm_traj_srv", req_q, res_q))
+ if(ros::service::call("TrajectoryQuery", req_q, res_q))
{
ROS_INFO("response:: %f, %d",res_q.trajectorytime,res_q.done);
}
- sleep(10);
- if(ros::service::call("query_arm_traj_srv", req_q, res_q))
+ else
+ {
+ ROS_INFO("service call failed");
+ }
+ sleep(4);
+ if(ros::service::call("TrajectoryQuery", req_q, res_q))
{
ROS_INFO("response:: %f, %d",res_q.trajectorytime,res_q.done);
}
+ else
+ {
+ ROS_INFO("service call failed");
+ }
}
Modified: pkg/trunk/robot_descriptions/arm_cart/run_trajectory_controller.launch
===================================================================
--- pkg/trunk/robot_descriptions/arm_cart/run_trajectory_controller.launch 2009-01-09 01:06:35 UTC (rev 9097)
+++ pkg/trunk/robot_descriptions/arm_cart/run_trajectory_controller.launch 2009-01-09 01:26:53 UTC (rev 9098)
@@ -1,7 +1,7 @@
<launch>
<!-- Arm trajectory controller -->
- <param name="right_arm_controller/velocity_scaling_factor" type="double" value="0.25"/>
+ <param name="right_arm_trajectory_controller/velocity_scaling_factor" type="double" value="0.25"/>
<node pkg="mechanism_control" type="spawner.py" args="$(find arm_cart)/trajectory_controllers.xml" />
Modified: pkg/trunk/robot_descriptions/arm_cart/trajectory_controllers.xml
===================================================================
--- pkg/trunk/robot_descriptions/arm_cart/trajectory_controllers.xml 2009-01-09 01:06:35 UTC (rev 9097)
+++ pkg/trunk/robot_descriptions/arm_cart/trajectory_controllers.xml 2009-01-09 01:26:53 UTC (rev 9098)
@@ -3,7 +3,7 @@
<controllers>
<!-- ========================================= -->
<!-- right arm array -->
- <controller name="right_arm_controller" type="ArmTrajectoryControllerNode">
+ <controller name="right_arm_trajectory_controller" type="ArmTrajectoryControllerNode">
<listen_topic name="arm_trajectory_command" />
<kinematics>
<elem key="kdl_chain_name">right_arm</elem>
@@ -49,6 +49,11 @@
</joint>
</controller>
+ <controller name="r_gripper_l_finger_controller" type="JointPDController">
+ <joint name="r_gripper_l_finger_joint">
+ <pid p="10" i="0.1" d="0.5" iClamp="10.0" />
+ </joint>
+ </controller>
<trajectory interpolation="cubic" />
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
