Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[13064] pkg/trunk
|
From: <stu...@us...> - 2009-03-27 22:47:01
|
Revision: 13064
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13064&view=rev
Author: stuglaser
Date: 2009-03-27 22:46:54 +0000 (Fri, 27 Mar 2009)
Log Message:
-----------
More plugging in
Modified Paths:
--------------
pkg/trunk/demos/plug_in/move_to_pickup.py
pkg/trunk/demos/plug_in/outlet_pose_filter.py
pkg/trunk/demos/plug_in/plug_in2.py
pkg/trunk/demos/plug_in/src/servo.cpp
pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/head_defs.xml
Modified: pkg/trunk/demos/plug_in/move_to_pickup.py
===================================================================
--- pkg/trunk/demos/plug_in/move_to_pickup.py 2009-03-27 22:27:12 UTC (rev 13063)
+++ pkg/trunk/demos/plug_in/move_to_pickup.py 2009-03-27 22:46:54 UTC (rev 13064)
@@ -210,7 +210,8 @@
pickup()
pub.publish(Float64(0.6))
- rospy.spin()
+ sleep(2)
+ #rospy.spin()
finally:
for name in controllers:
Modified: pkg/trunk/demos/plug_in/outlet_pose_filter.py
===================================================================
--- pkg/trunk/demos/plug_in/outlet_pose_filter.py 2009-03-27 22:27:12 UTC (rev 13063)
+++ pkg/trunk/demos/plug_in/outlet_pose_filter.py 2009-03-27 22:46:54 UTC (rev 13064)
@@ -79,6 +79,7 @@
tf_msg.transforms[0].header.stamp = rospy.rostime.get_rostime()
tf_pub.publish(tf_msg)
seq += 1
+ outlet_msg = track_outlet_pose.msg
else:
outlet_msg = track_outlet_pose.msg
Modified: pkg/trunk/demos/plug_in/plug_in2.py
===================================================================
--- pkg/trunk/demos/plug_in/plug_in2.py 2009-03-27 22:27:12 UTC (rev 13063)
+++ pkg/trunk/demos/plug_in/plug_in2.py 2009-03-27 22:46:54 UTC (rev 13064)
@@ -138,6 +138,8 @@
else:
# trajectory controller is already spawned
+ # TODO: actually check if the traj controller is up. Spawn if not
+
p_up = PoseStamped()
p_up.header.frame_id = 'base_link'
p_up.header.stamp = rospy.get_rostime()
@@ -150,19 +152,26 @@
p_face.pose.position = xyz(0.33, -0.09, 0.37)
p_face.pose.orientation = Quaternion(-0.04, 0.26, -0.00, 0.96)
- p_stage = PoseStamped()
- p_stage.header.frame_id = 'outlet_pose'
- p_stage.header.stamp = rospy.get_rostime()
- p_stage.pose.position = xyz(-0.12, 0.0, 0.0)
- p_stage.pose.orientation = rpy(0,0.3,0)
+ p_stage1 = PoseStamped()
+ p_stage1.header.frame_id = 'outlet_pose'
+ p_stage1.header.stamp = rospy.get_rostime()
+ p_stage1.pose.position = xyz(-0.12, 0.0, 0.04)
+ p_stage1.pose.orientation = rpy(0,0.3,0)
+ p_stage2 = PoseStamped()
+ p_stage2.header.frame_id = 'outlet_pose'
+ p_stage2.header.stamp = rospy.get_rostime()
+ p_stage2.pose.position = xyz(-0.07, 0.0, 0.04)
+ p_stage2.pose.orientation = rpy(0,0.3,0)
+
try:
print "Waiting for the trajectory controller"
move_arm = rospy.ServiceProxy('cartesian_trajectory_right/move_to', MoveToPose)
print "Staging the plug"
- move_arm(p_up)
- move_arm(p_face)
- move_arm(p_stage)
+ p_up.header.stamp = rospy.get_rostime(); move_arm(p_up)
+ p_face.header.stamp = rospy.get_rostime(); move_arm(p_face)
+ p_stage1.header.stamp = rospy.get_rostime(); move_arm(p_stage1)
+ p_stage2.header.stamp = rospy.get_rostime(); move_arm(p_stage2)
except rospy.ServiceException, e:
print "move_to service failed: %s" % e
@@ -252,7 +261,8 @@
rospy.wait_for_service("/arm_hybrid/set_tool_frame")
if rospy.is_shutdown(): return
set_tool_frame = rospy.ServiceProxy("/arm_hybrid/set_tool_frame", SetPoseStamped)
- set_tool_frame(plug_pose)
+ #set_tool_frame(plug_pose)
+ set_tool_frame(track_plug_pose.msg)
print "Tool frame set"
time.sleep(1)
Modified: pkg/trunk/demos/plug_in/src/servo.cpp
===================================================================
--- pkg/trunk/demos/plug_in/src/servo.cpp 2009-03-27 22:27:12 UTC (rev 13063)
+++ pkg/trunk/demos/plug_in/src/servo.cpp 2009-03-27 22:46:54 UTC (rev 13064)
@@ -37,13 +37,15 @@
#include "robot_msgs/TaskFrameFormalism.h"
const double MIN_STANDOFF = 0.035;
+const double SUCCESS_THRESHOLD = 0.025;
-enum {MEASURING, MOVING, PUSHING, FORCING};
+enum {MEASURING, MOVING, PUSHING, FORCING, HOLDING};
int g_state = MEASURING;
ros::Time g_started_pushing = ros::Time::now(),
g_started_forcing = ros::Time::now(),
- g_stopped_forcing = ros::Time::now();
+ g_stopped_forcing = ros::Time::now(),
+ g_started_holding = ros::Time::now();
boost::scoped_ptr<tf::MessageNotifier<robot_msgs::PoseStamped> > g_mn;
boost::scoped_ptr<tf::TransformListener> TF;
@@ -135,7 +137,7 @@
printf("Offset: (% 0.3lf, % 0.3lf, % 0.3lf)\n", offset.x(), offset.y(), offset.z());
if (g_started_pushing + ros::Duration(5.0) < ros::Time::now())
{
- if (false) // if (in_outlet)
+ if (offset.x() > SUCCESS_THRESHOLD) // if (in_outlet)
{
g_state = FORCING;
}
@@ -148,10 +150,16 @@
}
case FORCING: {
- g_state = MEASURING;
+ if (ros::Time::now() > g_started_forcing + ros::Duration(1.0))
+ g_state = HOLDING;
break;
}
+
+ case HOLDING: {
+ //g_state = MEASURING;
+ break;
}
+ }
// Handles the transitions
@@ -234,7 +242,7 @@
tff.mode.rot.x = 2;
tff.mode.rot.y = 2;
tff.mode.rot.z = 2;
- tff.value.vel.x = 40;
+ tff.value.vel.x = 50;
tff.value.vel.y = mech_offset.getOrigin().y();
tff.value.vel.z = mech_offset.getOrigin().z();
tff.value.rot.x = 0.0;
@@ -249,7 +257,27 @@
ros::Node::instance()->publish("/arm_hybrid/command", tff);
break;
}
+
+ case HOLDING: {
+ printf("enter HOLDING\n");
+ g_started_holding = ros::Time::now();
+ robot_msgs::TaskFrameFormalism tff;
+ tff.header.frame_id = "outlet_pose";
+ tff.header.stamp = msg->header.stamp;
+ tff.mode.vel.x = 1;
+ tff.mode.vel.y = 3;
+ tff.mode.vel.z = 3;
+ tff.mode.rot.x = 3;
+ tff.mode.rot.y = 3;
+ tff.mode.rot.z = 3;
+ tff.value.vel.x = 4;
+ tff.value.vel.y = mech_offset.getOrigin().y();
+ tff.value.vel.z = mech_offset.getOrigin().z();
+ mech_offset.getBasis().getEulerZYX(tff.value.rot.z, tff.value.rot.y, tff.value.rot.x);
+ ros::Node::instance()->publish("/arm_hybrid/command", tff);
+ break;
}
+ }
}
last_standoff = standoff;
Modified: pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/head_defs.xml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/head_defs.xml 2009-03-27 22:27:12 UTC (rev 13063)
+++ pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/head_defs.xml 2009-03-27 22:46:54 UTC (rev 13064)
@@ -299,7 +299,7 @@
<axis xyz="0 1 0" />
<limit min="-0.55" max="1.047" effort="12.21"
velocity="100" k_velocity="1.5"
- safety_length_min="0.02" safety_length_max="0.1" k_position="100" />
+ safety_length_min="0.02" safety_length_max="0.02" k_position="100" />
<calibration reference_position="${-0.195+cal_head_tilt_flag}" values="0 0" />
<joint_properties damping="1.0" />
</joint>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
