def __init__(self):
self.seq = 0
self.tf_pub = tf.TransformBroadcaster()
self.orange1 = (0.641782207489, -0.224464386702, -0.363829042912)
self.orange2 = (0.69, -0.31, -0.363829042912)
self.orange3 = (0.68, -0.10, -0.363829042912)
self.blue1 = (0.641782207489, -0.224464386702, -0.363829042912)
self.red1 = (0.69, -0.31, -0.363829042912)
self.green1 = (0.68, -0.10, -0.363829042912)
self.yellow1 = (0.68, -0.20, -0.363829042912)
table_pos = (0.5, 0., 0.863 - 0.5)
self.table_pos, self.table_rot = pm.toTf(
kdl.Frame(kdl.Rotation.RotZ(-np.pi / 2.), kdl.Vector(*table_pos)))
box_pos = (0.82, -0.4, 0.863 - 0.5 + 0.1025)
self.box_pos, self.box_rot = pm.toTf(
kdl.Frame(kdl.Rotation.RotZ(1.5), kdl.Vector(*box_pos)))
box_pos = (0.82, -0.4, 0.863 - 0.5 + 0.1025)
self.box_pos, self.box_rot = pm.toTf(
kdl.Frame(kdl.Rotation.RotZ(1.5), kdl.Vector(*box_pos)))
b1_pos = (0.78, -0.03, 0.863 - 0.5 + 0.0435)
self.block1_pos, self.block1_rot = pm.toTf(
kdl.Frame(kdl.Rotation.RotZ(-np.pi / 2.), kdl.Vector(*b1_pos)))
b1_pos = (0.73, 0.12, 0.863 - 0.5 + 0.0435)
self.block2_pos, self.block2_rot = pm.toTf(
kdl.Frame(kdl.Rotation.RotZ(-np.pi / 2. + 0.07),
kdl.Vector(*b1_pos)))
def test_custom_reference_relative_move(self):
""" Test if relative moves work with custom reference frame as expected
Test sequence:
1. Get the test data and publish reference frame via TF.
2. Create a relative Ptp with and without custom reference.
3. convert the goals.
4. Evaluate the results.
"""
rospy.loginfo("test_custom_reference_frame_relative")
self.robot.move(Ptp(goal=[0, 0, 0, 0, 0, 0]))
# get and transform test data
ref_frame = self.test_data.get_pose("Blend_1_Mid", PLANNING_GROUP_NAME)
goal_pose_bf = self.test_data.get_pose("Blend_1_Start", PLANNING_GROUP_NAME)
goal_pose_bf_tf = pm.toTf(pm.fromMsg(goal_pose_bf))
ref_frame_tf = pm.toTf(pm.fromMsg(ref_frame))
rospy.sleep(rospy.Duration.from_sec(0.5))
# init
base_frame_name = self.robot._robot_commander.get_planning_frame()
time_tf = rospy.Time.now()
goal_pose_in_rf = [[0, 0, 0], [0, 0, 0, 1]]
try:
self.tf.sendTransform(goal_pose_bf_tf[0], goal_pose_bf_tf[1], time_tf,
"rel_goal_pose_bf", base_frame_name)
self.tf.sendTransform(ref_frame_tf[0], ref_frame_tf[1], time_tf,
"ref_rel_frame", base_frame_name)
self.tf_listener.waitForTransform("rel_goal_pose_bf", "ref_rel_frame", time_tf, rospy.Duration(2, 0))
rospy.sleep(rospy.Duration.from_sec(0.1))
goal_pose_in_rf = self.tf_listener.lookupTransform("ref_rel_frame", "rel_goal_pose_bf", time_tf)
except:
rospy.logerr("Failed to setup transforms for test!")
goal_pose_rf_msg = pm.toMsg(pm.fromTf(goal_pose_in_rf))
# move to initial position and use relative move to reach goal
self.robot.move(Ptp(goal=ref_frame))
self.tf.sendTransform(ref_frame_tf[0], ref_frame_tf[1], rospy.Time.now(), "ref_rel_frame", base_frame_name)
rospy.sleep(rospy.Duration.from_sec(0.1))
self.tf_listener.waitForTransform(base_frame_name, "ref_rel_frame", rospy.Time(0), rospy.Duration(1, 0))
ptp = Ptp(goal=goal_pose_rf_msg, reference_frame="ref_rel_frame", relative=True)
req = ptp._cmd_to_request(self.robot)
self.assertIsNotNone(req)
self._analyze_request_pose(TARGET_LINK_NAME, goal_pose_bf, req)
def tick(self):
'''
Look for all transforms in the list
'''
if not self.listener.frameExists(self.root):
rospy.logwarn("%s was missing" % self.root)
return
count = 0
avg_p = np.zeros(3)
avg_q = np.zeros(4)
for name, pose in self.transforms.items():
if self.listener.frameExists(name):
t = self.listener.getLatestCommonTime(self.root, name)
p, q = self.listener.lookupTransform(self.root, name, t)
F = pm.fromTf((p, q))
F = F * pose
p, q = pm.toTf(F)
avg_p += np.array(p)
avg_q += np.array(q)
count += 1
if count > 0:
avg_p /= count
avg_q /= count
avg_q /= np.linalg.norm(avg_q)
if self.history_length > 0:
# use history to smooth predictions
if len(self.history) >= self.history_length:
self.history.pop()
self.history.appendleft((avg_p, avg_q))
avg_p = np.zeros(3)
avg_q = np.zeros(4)
for p, q in self.history:
avg_p += p
avg_q += q
avg_p /= len(self.history)
avg_q /= len(self.history)
avg_q /= np.linalg.norm(avg_q)
if self.offset is not None:
# apply some offset after we act
F = pm.fromTf((avg_p, avg_q))
F = F * self.offset
avg_p, avg_q = pm.toTf(F)
self.broadcaster.sendTransform(avg_p, avg_q, rospy.Time.now(),
self.name, self.root)
def update_tf(self):
for key in self.waypoints.keys():
(poses,names) = self.get_waypoints_srv.get_waypoints(key,[],self.waypoints[key],self.waypoint_names[key])
for (pose,name) in zip(poses,names):
(trans,rot) = pm.toTf(pm.fromMsg(pose))
self.broadcaster.sendTransform(trans,rot,rospy.Time.now(),name,self.world)
def publish_grasp_tran(tran):
"""@brief - Add a grasp relative to the world origin to the scene graph
@param tran - the grasp transform.,
"""
bc = tf.TransformBroadcaster()
ttf = pm.toTf(pm.fromMatrix(tran))
bc.sendTransform(ttf[0], ttf[1], rospy.Time.now(), "/hand_goal_pose", "/world")
def processMarkerFeedback(feedback):
global marker_tf, marker_name
marker_name = feedback.marker_name
marker_offset_tf = ((0, 0, marker_offset), tf.transformations.quaternion_from_euler(0, 0, 0))
marker_tf = pm.toTf(pm.fromMatrix(numpy.dot(pm.toMatrix(pm.fromMsg(feedback.pose)), pm.toMatrix(pm.fromTf(marker_offset_tf)))))
if feedback.menu_entry_id:
marker_tf = zero_tf
def pub_graspit_grasp_tf(self, object_name, graspit_grasp_msg):
# type: (str, graspit_msgs.msg.Grasp) -> ()
# Is this needed anymore?
tf_pose = pm.toTf(pm.fromMsg(graspit_grasp_msg.final_grasp_pose))
self.tf_broadcaster.sendTransform(tf_pose[0], tf_pose[1],
rospy.Time.now(),
"/grasp_approach_tran", object_name)
def step(self):
if self.T_ is None:
self.init_tf()
return
if self.recv_:
self.recv_ = False
pose = self.odom_.pose.pose
T0 = pm.toMatrix(pm.fromMsg(pose)) # android_odom --> android
# send odom -> base_link transform
T = tf.transformations.concatenate_matrices(self.T_, T0, self.Ti_)
frame = pm.fromMatrix(T)
if self.pub_tf_:
txn, qxn = pm.toTf(frame)
self.tfb_.sendTransform(txn, qxn, self.odom_.header.stamp,
'odom', 'base_link')
# send as msg
# TODO : does not deal with twist/covariance information
msg = pm.toMsg(frame)
self.cvt_msg_.pose.pose = pm.toMsg(frame)
self.cvt_msg_.header.frame_id = 'map' # experimental
self.cvt_msg_.header.stamp = self.odom_.header.stamp
self.pub_.publish(self.cvt_msg_)
def process_grasp_msg(self, grasp_msg):
"""@brief - Attempt to make the adjustment specified by grasp_msg
1. plan a path to the a place 15cm back from the new pose
"""
print 'regular move:'
print grasp_msg
#release the object
tp.open_barrett()
#get the robot's current location and calculate the absolute tran after the adjustment
#update the robot status
tp.update_robot(self.global_data.or_env.GetRobots()[0])
handGoalInBase = pm.toMatrix(pm.fromMsg(grasp_msg.final_grasp_pose))
try:
hand_tf = pm.toTf(pm.fromMatrix(handGoalInBase))
bc = tf.TransformBroadcaster()
now = rospy.Time.now()
bc.sendTransform(hand_tf[0], hand_tf[1], now, "hand_goal",
"armbase")
self.global_data.listener.waitForTransform("armbase",
"arm_goal", now,
rospy.Duration(1.0))
armtip_robot = self.global_data.listener.lookupTransform(
'armbase', 'arm_goal', now)
armGoalInBase = pm.toMatrix(pm.fromTf(armtip_robot))
except Exception, e:
handle_fatal('convert_cartesian_world_goal failed: error %s.' % e)
def pub_moveit_grasp_tf(self, object_name, moveit_grasp_msg):
# type: (str, moveit_msgs.msg.Grasp) -> ()
tf_pose = pm.toTf(pm.fromMsg(moveit_grasp_msg.grasp_pose.pose))
self.tf_broadcaster.sendTransform(tf_pose[0], tf_pose[1],
rospy.Time.now(),
"/moveit_end_effector_frame",
object_name)
def publish_grasp_tran(tran):
"""@brief - Add a grasp relative to the world origin to the scene graph
@param tran - the grasp transform.,
"""
bc = tf.TransformBroadcaster()
ttf = pm.toTf(pm.fromMatrix(tran))
bc.sendTransform(ttf[0], ttf[1], rospy.Time.now(), "/hand_goal_pose",
"/world")
def publish_tf(self):
for key in self.waypoints.keys():
(poses, names) = self.get_waypoints_srv.get_waypoints(
key, [], self.waypoints[key], self.waypoint_names[key])
for (pose, name) in zip(poses, names):
(trans, rot) = pm.toTf(pm.fromMsg(pose))
self.broadcaster.sendTransform(trans, rot, rospy.Time.now(),
name, self.world)
def broadcast_table_frame(args):
if table_pose is None:
return
with tf_lock:
trans, rot = pm.toTf(pm.fromMsg(table_pose.pose))
br.sendTransform(trans, rot, rospy.Time.now(), "/table", table_pose.header.frame_id)
br.sendTransform(trans, rot, rospy.Time.now() + rospy.Duration(0.005), "/table", table_pose.header.frame_id)
br.sendTransform(trans, rot, rospy.Time.now() - rospy.Duration(0.005), "/table", table_pose.header.frame_id)
def test_current_pose(self):
""" Test the current pose method
1. create and publish tf
2. get current pose with base and ref
3. move roboter to ref
4. get current pose with ref and base
5. analyse positions
"""
rospy.loginfo("test_current_pose")
self.robot.move(Ptp(goal=[0, 0, 0, 0, 0, 0]))
# get and transform test data
ref_frame = self.test_data.get_pose("Blend_1_Mid", PLANNING_GROUP_NAME)
ref_frame_tf = pm.toTf(pm.fromMsg(ref_frame))
rospy.sleep(rospy.Duration.from_sec(0.5))
# init
tcp_ref = [[0, 0, 0], [0, 0, 0, 1]]
tcp_base = [[0, 0, 0], [0, 0, 0, 1]]
time_tf = rospy.Time.now()
base_frame = self.robot._robot_commander.get_planning_frame()
try:
self.tf.sendTransform(ref_frame_tf[0], ref_frame_tf[1],
time_tf, "ref_frame", base_frame)
self.tf_listener.waitForTransform(base_frame, "ref_frame", time_tf, rospy.Duration(2, 0))
rospy.sleep(rospy.Duration.from_sec(0.1))
tcp_ref = self.tf_listener.lookupTransform("ref_frame", "prbt_tcp", time_tf)
tcp_base = self.tf_listener.lookupTransform(base_frame, "prbt_tcp", time_tf)
except Exception as e:
print e
rospy.logerr("Failed to setup transforms for test!")
tcp_ref_msg = pm.toMsg(pm.fromTf(tcp_ref))
tcp_base_msg = pm.toMsg(pm.fromTf(tcp_base))
# read current pose, move robot and do it again
start_bf = self.robot.get_current_pose()
start_rf = self.robot.get_current_pose(base="ref_frame")
self.robot.move(Ptp(goal=ref_frame))
# resending tf (otherwise transform pose could read old transform to keep time close together.
time_tf = rospy.Time.now()
self.tf.sendTransform(ref_frame_tf[0], ref_frame_tf[1], time_tf, "ref_frame", base_frame)
self.tf_listener.waitForTransform(base_frame, "ref_frame", time_tf, rospy.Duration(1, 0))
ref_frame_bf = self.robot.get_current_pose()
ref_frame_rf = self.robot.get_current_pose(base="ref_frame")
self._analyze_pose(tcp_base_msg, start_bf)
self._analyze_pose(tcp_ref_msg, start_rf)
self._analyze_pose(ref_frame, ref_frame_bf)
self._analyze_pose(Pose(orientation=Quaternion(0, 0, 0, 1)), ref_frame_rf)
def publish_target_pointcloud(self):
self.model_list.sort(key=ModelManager.get_dist)
x = self.model_list[0]
tf_pose = pm.toTf(pm.fromMsg(x.pose))
x.bc.sendTransform(tf_pose[0], tf_pose[1], rospy.Time.now(), "/object", "/camera_rgb_optical_frame")
x.listener.waitForTransform("/world", "/object", rospy.Time(0), rospy.Duration(5))
x.point_cloud_data.header.frame_id = "/object"
pub = rospy.Publisher('/object_pointcloud', sensor_msgs.msg.PointCloud2)
pub.publish(x.point_cloud_data)
def add_tf(self, frame_name, ps):
"""
:type frame_name: str
:type ps: geometry_msgs.msg.PoseStamped
:param frame_name: the name of the new frame
:param ps: the pose stamped for the new frame
:return: None
"""
translation, rotation = pm.toTf(pm.fromMsg(ps.pose))
self._tfs[frame_name] = (ps, translation, rotation)
def test_roundtrip(self):
c = Frame()
d = pm.fromMsg(pm.toMsg(c))
self.assertEqual(repr(c), repr(d))
d = pm.fromMatrix(pm.toMatrix(c))
self.assertEqual(repr(c), repr(d))
d = pm.fromTf(pm.toTf(c))
self.assertEqual(repr(c), repr(d))
def test_roundtrip(self):
c = Frame()
d = pm.fromMsg(pm.toMsg(c))
self.assertEqual(repr(c), repr(d))
d = pm.fromMatrix(pm.toMatrix(c))
self.assertEqual(repr(c), repr(d))
d = pm.fromTf(pm.toTf(c))
self.assertEqual(repr(c), repr(d))
def broadcast_table_frame(args):
if table_pose is None: return
with tf_lock:
trans, rot = pm.toTf(pm.fromMsg(table_pose.pose))
br.sendTransform(trans, rot, rospy.Time.now(), '/table',
table_pose.header.frame_id)
br.sendTransform(trans, rot,
rospy.Time.now() + rospy.Duration(0.005), '/table',
table_pose.header.frame_id)
br.sendTransform(trans, rot,
rospy.Time.now() - rospy.Duration(0.005), '/table',
table_pose.header.frame_id)
def joy_cb(self, msg):
# Convenience
side = self.side
b = msg.buttons
lb = self.last_buttons
if (rospy.Time.now() - self.last_hand_cmd) < rospy.Duration(0.1):
return
# Update enabled fingers
self.move_f[0] = self.move_f[0] ^ (b[self.B1[side]] and not lb[self.B1[side]])
self.move_f[1] = self.move_f[1] ^ (b[self.B2[side]] and not lb[self.B2[side]])
self.move_f[2] = self.move_f[2] ^ (b[self.B3[side]] and not lb[self.B3[side]])
self.move_spread = self.move_spread ^ (b[self.B4[side]] and not lb[self.B4[side]])
self.move_all = self.move_all ^ (b[self.B_CENTER[side]] and not lb[self.B_CENTER[side]])
# Check if the deadman is engaged
if msg.axes[self.DEADMAN[side]] < 0.01:
if self.deadman_engaged:
self.hand_cmd.mode = [oro_barrett_msgs.msg.BHandCmd.MODE_VELOCITY] * 4
self.hand_cmd.cmd = [0.0, 0.0, 0.0, 0.0]
self.hand_pub.publish(self.hand_cmd)
self.last_hand_cmd = rospy.Time.now()
self.deadman_engaged = False
self.deadman_max = 0.0
else:
self.handle_hand_cmd(msg)
self.handle_cart_cmd(msg)
self.last_buttons = msg.buttons
self.last_axes = msg.axes
# Broadcast the command if it's defined
if self.cmd_frame:
# Broadcast command frame
tform = toTf(self.cmd_frame)
self.broadcaster.sendTransform(tform[0], tform[1], rospy.Time.now(), self.cmd_frame_id, 'world')
# Broadcast telemanip command
telemanip_cmd = TelemanipCommand()
telemanip_cmd.header.frame_id = 'world'
telemanip_cmd.header.stamp = rospy.Time.now()
telemanip_cmd.posetwist.pose = toMsg(self.cmd_frame)
telemanip_cmd.resync_pose = msg.buttons[self.THUMB_CLICK[side]] == 1
telemanip_cmd.deadman_engaged = self.deadman_engaged
if msg.axes[self.THUMB_Y[side]] > 0.5:
telemanip_cmd.grasp_opening = 0.0
elif msg.axes[self.THUMB_Y[side]] < -0.5:
telemanip_cmd.grasp_opening = 1.0
else:
telemanip_cmd.grasp_opening = 0.5
telemanip_cmd.estop = False # TODO: add master estop control
self.telemanip_cmd_pub.publish(telemanip_cmd)
def compute_net_transform(self, base_pose):
base_to_odom = self.tfl.lookupTransform("/base_footprint", "/odom_combined", rospy.Time())
bto = pm.fromTf(base_to_odom)
net_t = base_pose * bto
print "Setting "
print pm.toMsg(net_t)
self.broad.transform = pm.toTf(net_t)
def thread_update_pose(self):
# update robot pose
rate = rospy.Rate(30)
while not rospy.is_shutdown():
tf = pm.toTf(self.frame)
self.br.sendTransform(tf[0],
tf[1],
rospy.Time.now(),
self.cam_frame,
self.ref_frame)
rate.sleep()
pass
def publish_target_pointcloud(self):
self.model_list.sort(key=ModelManager.get_dist)
x = self.model_list[0]
tf_pose = pm.toTf(pm.fromMsg(x.pose))
x.bc.sendTransform(tf_pose[0], tf_pose[1], rospy.Time.now(), "/object",
"/camera_rgb_optical_frame")
x.listener.waitForTransform("/world", "/object", rospy.Time(0),
rospy.Duration(5))
x.point_cloud_data.header.frame_id = "/object"
pub = rospy.Publisher('/object_pointcloud',
sensor_msgs.msg.PointCloud2)
pub.publish(x.point_cloud_data)
def publish_poses(self):
while not rospy.is_shutdown():
try:
self._F_left_gripper_base_left_gripper = posemath.fromTf(
self._tf_listener.lookupTransform('left_gripper_base',
'left_gripper',
rospy.Time(0)))
self._F_right_gripper_base_right_gripper = posemath.fromTf(
self._tf_listener.lookupTransform('right_gripper_base',
'right_gripper',
rospy.Time(0)))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.logwarn(
'[bin_pose_publisher]: haven\'t received gripper frames')
rospy.sleep(1.0)
continue
for pose in self._bin_poses_tf:
F = posemath.fromTf(
(tuple(pose['translation']), tuple(pose['rotation'])))
# left arm
if pose['bin_pose_id'].rfind('left_arm') > 0:
translation, rotation = posemath.toTf(
F * self._F_left_gripper_base_left_gripper)
# right arm
else:
translation, rotation = posemath.toTf(
F * self._F_right_gripper_base_right_gripper)
self._tf_broadcaster.sendTransform(translation=translation,
rotation=rotation,
time=rospy.Time.now(),
child=pose['bin_pose_id'],
parent=pose['bin'])
self._rate.sleep()
def moveRelative(self, arm, x=0.0, y=0.0, z=0.0, rx=0.0, ry=0.0, rz=0.0, frameId=None,
interpolate=False, stepSize=0.02, execute=True, mustReachGoal=True,
controlMode='position_w_id', startConfig=None):
""" Move the end effector relative to its current pose.
@param arm - which arm (left_arm or right_arm)
@param x,y,z,rx,ry,rz - position and orientation (in roll, pitch, yaw)
@param frameId - relative to which frame to (None means EEF)
@param interpolate - if true, we attempt to move on a straight line in cartesian space
Note that this means, we can not use the roadmap.
@param stepSize - step size for straight line motion. The smaller the more accurate the
line at the cost of slower execution.
@param mustReachGoal - indicates whether MoveResult.Success should only be returned
if the goal pose can be reached. If false, the longest valid trajectory towards
the goal is returned. Note this option is only considered if interpolate=True.
@param startConfig - start configuration from where to plan from. If set, execute is set to
False, if not set the current configuration is used as start configuration.
"""
with self._lock:
self.checkProcessState()
if frameId is None:
frameId = self.getGripperFrameName(arm)
if startConfig is None:
startConfig = self.getCurrentConfiguration(arm)
globalPlanner = self.getGlobalPlanner(arm)
# express eef pose in frameId
eefPoseInBase = globalPlanner.computeForwardPositionKinematics(startConfig)
F_base_eef = kdl.Frame(kdl.Rotation.Quaternion(*eefPoseInBase[3:7]), kdl.Vector(*eefPoseInBase[0:3]))
F_base_frame = posemath.fromMsg(self.getTransform(frameId, 'base').pose)
F_frame_eef = F_base_frame.Inverse() * F_base_eef
F_rel = kdl.Frame(kdl.Rotation.RPY(rx, ry, rz), kdl.Vector(x, y, z))
F_frame_eef = F_rel * F_frame_eef
(position, rotation) = posemath.toTf(F_frame_eef)
goalPose = utils.ArgumentsCollector.createROSPose(position=position, rotation=rotation, frame_id=frameId)
convertedPoses = self._convertPosesToFrame([goalPose], goalFrame='base')
if interpolate:
self.moveHeadAside(arm)
moveGroup = self.getMoveGroup(arm)
(planningSuccess, trajList) = globalPlanner.planCartesianPath(startConfig=startConfig,
goalPose=convertedPoses[0],
stepSize=stepSize,
linkName=self.getGripperFrameName(arm),
mustReachGoal=mustReachGoal)
if not planningSuccess:
return (MoveResult.PlanFail, None)
elif not execute:
return (MoveResult.Success, trajList)
else:
return self._executeTrajectoryList(trajList, moveGroup, controlMode=controlMode)
return self.moveToPoses([goalPose], arm, startConfig=startConfig)
def publish_true_object_tran(height):
"""@brief - A debugging function to visualize the origin of the objects
in from openrave in the scene graph.
@param height - the offset from the table to the origin of the object.
"""
try:
object_height_offset = eye(4)
object_height_offset[2,3] = height
object_height_tf = pm.toTf(pm.fromMatrix(object_height_offset))
bc = tf.TransformBroadcaster()
bc.sendTransform(object_height_tf[0], object_height_tf[1], rospy.Time.now(), "/true_obj_pose", "/object")
except Exception,e:
rospy.logwarn("failed to publish true object height: %s"%e)
return []
def lookupTransform(self, frame_id, child_frame_id, time):
'''
@param frame_id:
@param child_frame_id:
@param time:
@return:
'''
with self._lock:
f_base_child = self._tfs.get(child_frame_id, None)
f_base_parent = self._tfs.get(frame_id, None)
gripper = child_frame_id.strip('_base')
if child_frame_id == 'left_gripper' or child_frame_id == 'right_gripper':
f_base_gripper = self._get_gripper_frame(gripper)
f_base_child = copy.deepcopy(f_base_gripper)
elif child_frame_id == 'left_gripper_base' or child_frame_id == 'right_gripper_base':
f_base_gripper = self._get_gripper_frame(gripper)
f_gripper_base_gripper = self._tfs.get(gripper)
f_base_child = copy.deepcopy(f_base_gripper *
f_gripper_base_gripper.Inverse())
gripper = frame_id.strip('_base')
if frame_id == 'left_gripper' or frame_id == 'right_gripper':
f_base_gripper = self._get_gripper_frame(gripper)
f_base_parent = copy.deepcopy(f_base_gripper)
elif frame_id == 'left_gripper_base' or frame_id == 'right_gripper_base':
f_base_gripper = self._get_gripper_frame(gripper)
f_gripper_base_gripper = self._tfs.get(gripper)
f_base_parent = copy.deepcopy(f_base_gripper *
f_gripper_base_gripper.Inverse())
if f_base_child == None or f_base_parent == None:
try:
position, orientation = self._tf_listener.lookupTransform(
frame_id, child_frame_id, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
raise tf.LookupException
return (position, orientation)
return posemath.toTf(f_base_parent.Inverse() * f_base_child)
def publish_true_object_tran(height):
"""@brief - A debugging function to visualize the origin of the objects
in from openrave in the scene graph.
@param height - the offset from the table to the origin of the object.
"""
try:
object_height_offset = eye(4)
object_height_offset[2, 3] = height
object_height_tf = pm.toTf(pm.fromMatrix(object_height_offset))
bc = tf.TransformBroadcaster()
bc.sendTransform(object_height_tf[0], object_height_tf[1],
rospy.Time.now(), "/true_obj_pose", "/object")
except Exception, e:
rospy.logwarn("failed to publish true object height: %s" % e)
return []
def lookupTransform(self, frame_id, child_frame_id, time):
"""
@param frame_id:
@param child_frame_id:
@param time:
@return:
"""
with self._lock:
f_base_child = self._tfs.get(child_frame_id, None)
f_base_parent = self._tfs.get(frame_id, None)
gripper = child_frame_id.strip("_base")
if child_frame_id == "left_gripper" or child_frame_id == "right_gripper":
f_base_gripper = self._get_gripper_frame(gripper)
f_base_child = copy.deepcopy(f_base_gripper)
elif child_frame_id == "left_gripper_base" or child_frame_id == "right_gripper_base":
f_base_gripper = self._get_gripper_frame(gripper)
f_gripper_base_gripper = self._tfs.get(gripper)
f_base_child = copy.deepcopy(f_base_gripper * f_gripper_base_gripper.Inverse())
gripper = frame_id.strip("_base")
if frame_id == "left_gripper" or frame_id == "right_gripper":
f_base_gripper = self._get_gripper_frame(gripper)
f_base_parent = copy.deepcopy(f_base_gripper)
elif frame_id == "left_gripper_base" or frame_id == "right_gripper_base":
f_base_gripper = self._get_gripper_frame(gripper)
f_gripper_base_gripper = self._tfs.get(gripper)
f_base_parent = copy.deepcopy(f_base_gripper * f_gripper_base_gripper.Inverse())
if f_base_child == None or f_base_parent == None:
try:
position, orientation = self._tf_listener.lookupTransform(frame_id, child_frame_id, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
raise tf.LookupException
return (position, orientation)
return posemath.toTf(f_base_parent.Inverse() * f_base_child)
def convert_cartesian_relative_goal(global_data, move_tran):
"""@brief - Convert a position relative to the hand's current coordinate system to the robot's base coordinate system.
@param move_tran - the relative position in the hand's coordinate system.
"""
try:
move_tf = pm.toTf(pm.fromMatrix(move_tran))
print move_tf
bc = tf.TransformBroadcaster()
now = rospy.Time.now()
bc.sendTransform(move_tf[0], move_tf[1], now, "hand_goal", "hand")
#listener = tf.TransformListener()
global_data.listener.waitForTransform("armbase", "arm_goal", now, rospy.Duration(1.0))
armtip_robot = global_data.listener.lookupTransform('armbase','arm_goal', now)
#arm_robot = global_data.listener.lookupTransform('armbase', 'arm', now)
return pm.toMatrix(pm.fromTf(armtip_robot))
except Exception, e:
handle_fatal('convert_cartesian_relative_goal failed: error %s.'%e)
def __init__(self,
marker="ar_marker_0",
camera_link="camera_link",
ee_marker="ee_marker",
base_link="base_link",
listener=None,
broadcaster=None):
if broadcaster is None:
self.broadcaster = tf.TransformBroadcaster()
else:
self.broadcaster = broadcaster
if listener is None:
self.listener = tf.TransformListener()
else:
self.listener = listener
self.ee_marker = ee_marker
self.base_link = base_link
self.marker = marker
self.camera_link = camera_link
self.id = "%s_to_%s" % (base_link, camera_link)
self.trans = (0, 0, 0)
self.rot = (0, 0, 0, 1)
self.srv = rospy.Service('calibrate', Empty, self.calibrate)
self.add_type_service = rospy.ServiceProxy('/librarian/add_type',
librarian_msgs.srv.AddType)
self.save_service = rospy.ServiceProxy('/librarian/save',
librarian_msgs.srv.Save)
self.load_service = rospy.ServiceProxy('/librarian/load',
librarian_msgs.srv.Load)
rospy.wait_for_service('/librarian/add_type')
self.add_type_service(type="handeye_calibration")
resp = self.load_service(type="handeye_calibration", id=self.id)
print resp
if resp.status.result > 0:
(self.trans, self.rot) = pm.toTf(pm.fromMsg(yaml.load(resp.text)))
def convert_cartesian_world_goal(global_data, world_tran):
"""@brief - Get armtip goal pose in the arms coordinate system from hand world goal pose.
This is useful to convert between a hand goal in the world coordinate system to a cartesian goal that
can be sent directly to the staubli controller.
@param world_tf - tf of hand goal pose in world coordinates
"""
try:
world_tf = pm.toTf(pm.fromMatrix(world_tran))
bc = tf.TransformBroadcaster()
now = rospy.Time.now()
bc.sendTransform(world_tf[0], world_tf[1], now, "hand_goal", "world")
#listener = tf.TransformListener()
global_data.listener.waitForTransform("armbase", "arm_goal", now, rospy.Duration(1.0))
armtip_robot = global_data.listener.lookupTransform('armbase', 'arm_goal', now)
return pm.toMatrix(pm.fromTf(armtip_robot))
except Exception, e:
handle_fatal('convert_cartesian_world_goal failed: error %s.'%e)
def query_cb(self,req):
list_of_waypoints = self.query(req)
if len(list_of_waypoints) == 0:
return "FAILURE"
else:
# set param and publish TF frame appropriately under reserved name
if self.last_query is not None and self.last_query == req:
# return idx + 1
self.last_query_idx += 1
if self.last_query_idx >= len(list_of_waypoints):
self.last_query_idx = 0
else:
self.last_query = req
self.last_query_idx = 0
dist,T,object_t,name = list_of_waypoints[self.last_query_idx]
self.query_frame = pm.toTf(T)
return "SUCCESS"
def computeAndSendRotations(self,obj_id,dim,num_symmetries,rotation,kdlRot):
names = []
angle = rotation
if angle == 0 and num_symmetries > 0:
angle = 2*np.pi / float(num_symmetries)
for i in range(2,num_symmetries):
# rotate the object
R = kdl.Frame(kdlRot(angle*(i)))
(trans,rot) = pm.toTf(R)
obj_symmetry_name = "%s/%s%d"%(obj_id,dim,i)
self.broadcaster.sendTransform(
(0,0,0),
rot,
rospy.Time.now(),
obj_symmetry_name,
obj_id)
names.append(obj_symmetry_name)
return names
def pubTF(pose, parentName, childName):
br = tf2_ros.TransformBroadcaster()
t = geometry_msgs.msg.TransformStamped()
t.header.stamp = rospy.Time.now()
t.header.frame_id = parentName
t.child_frame_id = childName
transform = posemath.toTf(pose)
t.transform.translation.x = transform[0][0]
t.transform.translation.y = transform[0][1]
t.transform.translation.z = transform[0][2]
t.transform.rotation.x = transform[1][0]
t.transform.rotation.y = transform[1][1]
t.transform.rotation.z = transform[1][2]
t.transform.rotation.w = transform[1][3]
br.sendTransform(t)
def calibrate(self, req):
print "%s <- %s, %s <- %s" % (self.camera_link, self.marker,
self.base_link, self.ee_marker)
T_cm = pm.fromTf(self.lookup(self.camera_link, self.marker))
T_be = pm.fromTf(self.lookup(self.base_link, self.ee_marker))
print T_cm
print T_be
T = T_cm * T_be.Inverse()
(self.trans, self.rot) = pm.toTf(T)
print(self.trans, self.rot)
print self.save_service(type="handeye_calibration",
id=self.id,
text=yaml.dump(pm.toMsg(T)))
return EmptyResponse()
def convert_cartesian_world_goal(global_data, world_tran):
"""@brief - Get armtip goal pose in the arms coordinate system from hand world goal pose.
This is useful to convert between a hand goal in the world coordinate system to a cartesian goal that
can be sent directly to the staubli controller.
@param world_tf - tf of hand goal pose in world coordinates
"""
try:
world_tf = pm.toTf(pm.fromMatrix(world_tran))
bc = tf.TransformBroadcaster()
now = rospy.Time.now()
bc.sendTransform(world_tf[0], world_tf[1], now, "hand_goal", "world")
#listener = tf.TransformListener()
global_data.listener.waitForTransform("armbase", "arm_goal", now,
rospy.Duration(1.0))
armtip_robot = global_data.listener.lookupTransform(
'armbase', 'arm_goal', now)
return pm.toMatrix(pm.fromTf(armtip_robot))
except Exception, e:
handle_fatal('convert_cartesian_world_goal failed: error %s.' % e)
def convert_cartesian_relative_goal(global_data, move_tran):
"""@brief - Convert a position relative to the hand's current coordinate system to the robot's base coordinate system.
@param move_tran - the relative position in the hand's coordinate system.
"""
try:
move_tf = pm.toTf(pm.fromMatrix(move_tran))
print move_tf
bc = tf.TransformBroadcaster()
now = rospy.Time.now()
bc.sendTransform(move_tf[0], move_tf[1], now, "hand_goal", "hand")
#listener = tf.TransformListener()
global_data.listener.waitForTransform("armbase", "arm_goal", now,
rospy.Duration(1.0))
armtip_robot = global_data.listener.lookupTransform(
'armbase', 'arm_goal', now)
#arm_robot = global_data.listener.lookupTransform('armbase', 'arm', now)
return pm.toMatrix(pm.fromTf(armtip_robot))
except Exception, e:
handle_fatal('convert_cartesian_relative_goal failed: error %s.' % e)
def publish_cmd(self, resync_pose, augmenter_engaged, grasp_opening, time):
"""publish the raw tf frame and the telemanip command"""
if not self.cmd_frame:
return
# Broadcast command frame
tform = toTf(self.cmd_frame)
self.broadcaster.sendTransform(tform[0], tform[1], time, self.cmd_frame_id, 'world')
# Broadcast telemanip command
telemanip_cmd = TelemanipCommand()
telemanip_cmd.header.frame_id = 'world'
telemanip_cmd.header.stamp = time
telemanip_cmd.posetwist.pose = toMsg(self.cmd_frame)
telemanip_cmd.resync_pose = resync_pose
telemanip_cmd.deadman_engaged = self.deadman_engaged
telemanip_cmd.augmenter_engaged = augmenter_engaged
telemanip_cmd.grasp_opening = grasp_opening
telemanip_cmd.estop = False # TODO: add master estop control
self.telemanip_cmd_pub.publish(telemanip_cmd)
def __init__(self,
marker="ar_marker_0",
camera_link="camera_link",
ee_marker="ee_marker",
base_link="base_link",
listener = None,
broadcaster = None):
if broadcaster is None:
self.broadcaster = tf.TransformBroadcaster()
else:
self.broadcaster = broadcaster
if listener is None:
self.listener = tf.TransformListener()
else:
self.listener = listener
self.ee_marker = ee_marker
self.base_link = base_link
self.marker = marker
self.camera_link = camera_link
self.id = "%s_to_%s"%(base_link,camera_link)
self.trans = (0,0,0)
self.rot = (0,0,0,1)
self.srv = rospy.Service('calibrate',Empty,self.calibrate)
self.add_type_service = rospy.ServiceProxy('/librarian/add_type', librarian_msgs.srv.AddType)
self.save_service = rospy.ServiceProxy('/librarian/save', librarian_msgs.srv.Save)
self.load_service = rospy.ServiceProxy('/librarian/load', librarian_msgs.srv.Load)
rospy.wait_for_service('/librarian/add_type')
self.add_type_service(type="handeye_calibration")
resp = self.load_service(type="handeye_calibration", id=self.id)
print resp
if resp.status.result > 0:
(self.trans, self.rot) = pm.toTf(pm.fromMsg(yaml.load(resp.text)))
def handle_tick(self):
br = tf.TransformBroadcaster()
br.sendTransform((0,0,0),tf.transformations.quaternion_from_euler(0,0,0),rospy.Time.now(),"/endpoint",self.end_link)
if not self.base_link == "base_link":
br.sendTransform((0,0,0),tf.transformations.quaternion_from_euler(0,0,0),rospy.Time.now(),"/base_link",self.base_link)
if self.query_frame is not None:
trans, rot = self.query_frame
br.sendTransform(trans, rot, rospy.Time.now(),self.query_frame_name,self.world)
if self.debug:
if self.backoff_waypoints is not None:
for tf_name, transform in self.backoff_waypoints:
trans, rot = pm.toTf(transform)
br.sendTransform(trans, rot, rospy.Time.now(),tf_name,self.world)
if self.table_frame is not None:
self.listener.waitForTransform(self.world, self.table_frame, rospy.Time(), rospy.Duration(1.0))
try:
self.table_pose = self.listener.lookupTransform(self.world, self.table_frame, rospy.Time(0))
except tf.ExtrapolationException, e:
rospy.logwarn(str(e))
def handle_tick(self):
br = tf.TransformBroadcaster()
br.sendTransform((0,0,0),tf.transformations.quaternion_from_euler(0,0,0),rospy.Time.now(),"/endpoint",self.end_link)
if not self.base_link == "base_link":
br.sendTransform((0,0,0),tf.transformations.quaternion_from_euler(0,0,0),rospy.Time.now(),"/base_link",self.base_link)
if self.query_frame is not None:
trans, rot = self.query_frame
br.sendTransform(trans, rot, rospy.Time.now(),self.query_frame_name,self.world)
if self.debug:
if self.backoff_waypoints is not None:
for tf_name, transform in self.backoff_waypoints:
trans, rot = pm.toTf(transform)
br.sendTransform(trans, rot, rospy.Time.now(),tf_name,self.world)
if self.table_frame is not None:
self.listener.waitForTransform(self.world, self.table_frame, rospy.Time(), rospy.Duration(1.0))
try:
self.table_pose = self.listener.lookupTransform(self.world, self.table_frame, rospy.Time(0))
except tf.ExtrapolationException, e:
rospy.logwarn(str(e))
def query_cb(self,req):
'''
Wrapper for the QUERY service. This gets the list of waypoints matching some criteria.
'''
list_of_waypoints = self.query(req)
if len(list_of_waypoints) == 0:
return "FAILURE"
else:
# set param and publish TF frame appropriately under reserved name
if self.last_query is not None and self.last_query == req:
# return idx + 1
self.last_query_idx += 1
if self.last_query_idx >= len(list_of_waypoints):
self.last_query_idx = 0
else:
self.last_query = req
self.last_query_idx = 0
dist,T,object_t,name = list_of_waypoints[self.last_query_idx]
self.query_frame = pm.toTf(T)
return "SUCCESS"
def calibrate(self,req):
print "%s <- %s, %s <- %s"%(self.camera_link,self.marker,self.base_link,self.ee_marker)
T_cm = pm.fromTf(self.lookup(self.camera_link,self.marker))
T_be = pm.fromTf(self.lookup(self.base_link,self.ee_marker))
print T_cm
print T_be
T = T_cm * T_be.Inverse()
(self.trans, self.rot) = pm.toTf(T)
print (self.trans, self.rot)
print self.save_service(
type="handeye_calibration",
id=self.id,
text=yaml.dump(pm.toMsg(T)))
return EmptyResponse()
def odom_cb(self, msg):
stamp = msg.header.stamp
try:
self._tfl.waitForTransform('base_footprint', 'odom', stamp, timeout=self._timeout)
o2b_p, o2b_q = self._tfl.lookupTransform('odom', 'base_footprint', stamp)#'base_footprint', 'odom', stamp)
except tf.Exception as e:
rospy.loginfo('w2o tf error : {}'.format(e))
return
o2b_T = tx.concatenate_matrices(
tx.translation_matrix(o2b_p), tx.quaternion_matrix(o2b_q)
)
o2b_T_i = tx.inverse_matrix(o2b_T)
w2b = msg.pose.pose
w2b_T = pm.toMatrix(pm.fromMsg(w2b))
w2o_T = tx.concatenate_matrices(w2b_T, o2b_T_i)
txn, rxn = pm.toTf(pm.fromMatrix(w2o_T))
self._tfb.sendTransform(txn, rxn, stamp, 'odom', 'world')
def process_grasp_msg(self, grasp_msg):
"""@brief - Attempt to make the adjustment specified by grasp_msg
1. plan a path to the a place 15cm back from the new pose
"""
print 'regular move:'
print grasp_msg
#release the object
tp.open_barrett()
#get the robot's current location and calculate the absolute tran after the adjustment
#update the robot status
tp.update_robot(self.global_data.or_env.GetRobots()[0])
handGoalInBase = pm.toMatrix(pm.fromMsg(grasp_msg.final_grasp_pose))
try:
hand_tf = pm.toTf(pm.fromMatrix(handGoalInBase))
bc = tf.TransformBroadcaster()
now = rospy.Time.now()
bc.sendTransform(hand_tf[0], hand_tf[1], now, "hand_goal", "armbase")
self.global_data.listener.waitForTransform("armbase", "arm_goal", now, rospy.Duration(1.0))
armtip_robot = self.global_data.listener.lookupTransform('armbase', 'arm_goal', now)
armGoalInBase = pm.toMatrix(pm.fromTf(armtip_robot))
except Exception, e:
handle_fatal('convert_cartesian_world_goal failed: error %s.'%e)
def __call__(self):
tf_pose = pm.toTf(pm.fromMsg(self.pose))
self.bc.sendTransform(tf_pose[0], tf_pose[1], rospy.Time.now(), self.object_name, "/camera_rgb_optical_frame")
def roll_rospose(x):
txn, qxn = pm.toTf(pm.fromMsg(x))
x,y,_ = txn
rz = tx.euler_from_quaternion(qxn)[-1]
res = [x,y,rz]
return res
def joy_cb(self, msg):
# Convenience
side = self.side
b = msg.buttons
lb = self.last_buttons
if (rospy.Time.now() - self.last_hand_cmd) < rospy.Duration(0.03):
return
# Update enabled fingers
self.move_f[0] = self.move_f[0] ^ (b[self.B1[side]] and not lb[self.B1[side]])
self.move_f[1] = self.move_f[1] ^ (b[self.B2[side]] and not lb[self.B2[side]])
self.move_f[2] = self.move_f[2] ^ (b[self.B3[side]] and not lb[self.B3[side]])
self.move_spread = self.move_spread ^ (b[self.B4[side]] and not lb[self.B4[side]])
self.move_all = self.move_all ^ (b[self.B_CENTER[side]] and not lb[self.B_CENTER[side]])
# Check if the deadman is engaged
if msg.axes[self.DEADMAN[side]] < 0.01:
if self.deadman_engaged:
self.hand_cmd.mode = [oro_barrett_msgs.msg.BHandCmd.MODE_VELOCITY] * 4
self.hand_cmd.cmd = [0.0, 0.0, 0.0, 0.0]
self.hand_pub.publish(self.hand_cmd)
self.last_hand_cmd = rospy.Time.now()
self.deadman_engaged = False
self.deadman_max = 0.0
else:
self.handle_hand_cmd(msg)
self.handle_cart_cmd(msg)
self.last_buttons = msg.buttons
self.last_axes = msg.axes
# republish markers
for i,m in enumerate(self.master_target_markers.markers):
m.header.stamp = rospy.Time.now()
if (i <3 and not self.move_f[i]) or (i==3 and not self.move_spread):
m.color = self.color_orange
elif not self.move_all:
m.color = self.color_red
else:
if self.deadman_engaged:
m.color = self.color_green
else:
m.color = self.color_gray
if i < 3:
if i != 2:
p = m.pose.position
s = (-1.0 if i == 0 else 1.0)
p.x, p.y, p.z = finger_point(
self.hand_position[3] * s,
l = 0.025 * s)
self.marker_pub.publish(self.master_target_markers)
# Broadcast the command if it's defined
if self.cmd_frame:
# Broadcast command frame
tform = toTf(self.cmd_frame)
self.broadcaster.sendTransform(tform[0], tform[1], rospy.Time.now(), self.cmd_frame_id, 'world')
# Broadcast telemanip command
telemanip_cmd = TelemanipCommand()
telemanip_cmd.header.frame_id = 'world'
telemanip_cmd.header.stamp = rospy.Time.now()
telemanip_cmd.posetwist.pose = toMsg(self.cmd_frame)
telemanip_cmd.resync_pose = msg.buttons[self.THUMB_CLICK[side]] == 1
telemanip_cmd.deadman_engaged = self.deadman_engaged
if msg.axes[self.THUMB_Y[side]] > 0.5:
telemanip_cmd.grasp_opening = 0.0
elif msg.axes[self.THUMB_Y[side]] < -0.5:
telemanip_cmd.grasp_opening = 1.0
else:
telemanip_cmd.grasp_opening = 0.5
telemanip_cmd.estop = False # TODO: add master estop control
self.telemanip_cmd_pub.publish(telemanip_cmd)
def broadcast_tf(self):
tf_pose = pm.toTf(pm.fromMsg(self.pose.pose))
self.bc.sendTransform(tf_pose[0], tf_pose[1], rospy.Time.now(), self.object_name, self.pose.header.frame_id)
def pub_graspit_grasp_tf(self, object_name, graspit_grasp_msg):
# type: (str, graspit_msgs.msg.Grasp) -> ()
# Is this needed anymore?
tf_pose = pm.toTf(pm.fromMsg(graspit_grasp_msg.final_grasp_pose))
self.tf_broadcaster.sendTransform(tf_pose[0], tf_pose[1], rospy.Time.now(), "/grasp_approach_tran", object_name)
r = kdl.Rotation()
for i in range(3):
for j in range(3):
if i == 0:
r[j,i] = x[j]
elif i == 1:
r[j,i] = y[j]
else:
r[j,i] = z[j]
t = F_obj_frame.p - F_bin_frame.p
f = kdl.Frame(r, t)
graspFrame = posemath.toTf(f)
return graspFrame
def publishGraspingFrame(bin_name, obj_name):
br = tf.TransformBroadcaster()
listener = tf.TransformListener()
r = rospy.Rate(10)
while not rospy.is_shutdown():
graspFrame = getGraspFrame(listener, bin_name, obj_name)
br.sendTransform(graspFrame[0], graspFrame[1], \
rospy.Time.now(), \
"/grasp_frame", \
listener = tf.TransformListener()
trans_list = []
rot_list = []
rate = rospy.Rate(10.0)
while not rospy.is_shutdown():
try:
(trans, rot) = listener.lookupTransform(sys.argv[1], sys.argv[2], rospy.Time(0))
(trans2, rot2) = listener.lookupTransform(sys.argv[3], sys.argv[4], rospy.Time(0))
msg1 = Pose()
msg2 = Pose()
msg1.position.x, msg1.position.y, msg1.position.z = trans[0], trans[1], trans[2]
msg2.position.x, msg2.position.y, msg2.position.z = trans2[0], trans2[1], trans2[2]
msg1.orientation.x, msg1.orientation.y, msg1.orientation.z, msg1.orientation.w = rot[0], rot[1], rot[2], rot[3]
msg2.orientation.x, msg2.orientation.y, msg2.orientation.z, msg2.orientation.w = rot2[0], rot2[1], rot2[2], rot2[3]
(trans_tot, rot_tot) = pm.toTf(pm.fromMsg(msg1)*pm.fromMsg(msg2))
print "translation: ", trans_tot, ", rotation :", rot_tot
trans_list.append(trans_tot)
rot_list.append(rot_tot)
except (tf.LookupException, tf.ConnectivityException):
continue
rate.sleep()
trans_str = str(np.median(np.array(trans_list), axis = 0))
rot_str = str(np.median(np.array(rot_list), axis = 0))
print "median of translation :", trans_str
print "median of rotation :", rot_str
try:
os.remove('../../launch/kinect_playpen_to_torso_lift_link.launch')
except:
def moveRelative(self,
arm,
x=0.0,
y=0.0,
z=0.0,
rx=0.0,
ry=0.0,
rz=0.0,
frameId=None,
interpolate=False,
stepSize=0.02,
execute=True,
mustReachGoal=True,
controlMode='position_w_id',
startConfig=None):
""" Move the end effector relative to its current pose.
@param arm - which arm (left_arm or right_arm)
@param x,y,z,rx,ry,rz - position and orientation (in roll, pitch, yaw)
@param frameId - relative to which frame to (None means EEF)
@param interpolate - if true, we attempt to move on a straight line in cartesian space
Note that this means, we can not use the roadmap.
@param stepSize - step size for straight line motion. The smaller the more accurate the
line at the cost of slower execution.
@param mustReachGoal - indicates whether MoveResult.Success should only be returned
if the goal pose can be reached. If false, the longest valid trajectory towards
the goal is returned. Note this option is only considered if interpolate=True.
@param startConfig - start configuration from where to plan from. If set, execute is set to
False, if not set the current configuration is used as start configuration.
"""
with self._lock:
self.checkProcessState()
if frameId is None:
frameId = self.getGripperFrameName(arm)
if startConfig is None:
startConfig = self.getCurrentConfiguration(arm)
globalPlanner = self.getGlobalPlanner(arm)
# express eef pose in frameId
eefPoseInBase = globalPlanner.computeForwardPositionKinematics(
startConfig)
F_base_eef = kdl.Frame(
kdl.Rotation.Quaternion(*eefPoseInBase[3:7]),
kdl.Vector(*eefPoseInBase[0:3]))
F_base_frame = posemath.fromMsg(
self.getTransform(frameId, 'base').pose)
F_frame_eef = F_base_frame.Inverse() * F_base_eef
F_rel = kdl.Frame(kdl.Rotation.RPY(rx, ry, rz),
kdl.Vector(x, y, z))
F_frame_eef = F_rel * F_frame_eef
(position, rotation) = posemath.toTf(F_frame_eef)
goalPose = utils.ArgumentsCollector.createROSPose(
position=position, rotation=rotation, frame_id=frameId)
convertedPoses = self._convertPosesToFrame([goalPose],
goalFrame='base')
if interpolate:
self.moveHeadAside(arm)
moveGroup = self.getMoveGroup(arm)
(planningSuccess, trajList) = globalPlanner.planCartesianPath(
startConfig=startConfig,
goalPose=convertedPoses[0],
stepSize=stepSize,
linkName=self.getGripperFrameName(arm),
mustReachGoal=mustReachGoal)
if not planningSuccess:
return (MoveResult.PlanFail, None)
elif not execute:
return (MoveResult.Success, trajList)
else:
return self._executeTrajectoryList(trajList,
moveGroup,
controlMode=controlMode)
return self.moveToPoses([goalPose], arm, startConfig=startConfig)
def pub_moveit_grasp_tf(self, object_name, moveit_grasp_msg):
# type: (str, moveit_msgs.msg.Grasp) -> ()
# Is this needed anymore?
tf_pose = pm.toTf(pm.fromMsg(moveit_grasp_msg.grasp_pose.pose))
self.tf_broadcaster.sendTransform(tf_pose[0], tf_pose[1], rospy.Time.now(), "/moveit_end_effector_frame", object_name)
def __call__(self):
tf_pose = pm.toTf(pm.fromMsg(self.pose))
self.bc.sendTransform(tf_pose[0], tf_pose[1], rospy.Time.now(),
self.object_name,
"/camera_rgb_optical_frame")
def publish_cartesian_waypoints(self):
for (name, pose) in self.cart_waypoints:
(trans, rot) = pm.toTf(pm.fromMsg(pose))
self.broadcaster.sendTransform(trans, rot, rospy.Time.now(), name,
self.world)
rospy.wait_for_service('getCartesian')
try:
get_cartesian = rospy.ServiceProxy( 'getCartesian', staubli_tx60.srv.GetCartesian )
resp1 = get_cartesian()
# make srv x y z rx ry rz euler angle representation into pose message
pos = geometry_msgs.msg.Point( x = resp1.x, y = resp1.y, z = resp1.z )
q = tf.transformations.quaternion_from_euler( resp1.rx , resp1.ry, resp1.rz ,'rxyz' )
quat = geometry_msgs.msg.Quaternion( x = q[0], y = q[1], z = q[2], w = q[3] )
return geometry_msgs.msg.Pose( position = pos, orientation = quat )
except rospy.ServiceException, e:
print "Service call failed: %s"%e
return []
def get_staubli_cartesian_as_tran():
current_pose = get_staubli_cartesian_as_pose_msg()
return pm.toMatrix(pm.fromMsg(current_pose))
rospy.init_node('arm_position_publisher')
tf_publisher = tf.TransformBroadcaster()
loop = rospy.Rate(3.0)
while not rospy.is_shutdown():
t = get_staubli_cartesian_as_tran()
t_tf = pm.toTf(pm.fromMatrix(t))
tf_publisher.sendTransform(t_tf[0], t_tf[1], rospy.Time.now(),"/arm","/armbase")
loop.sleep()
@author: Chris
"""
import rospy
import tf
import tf_conversions.posemath as pm
from geometry_msgs.msg import PoseArray
last_pose = None
def callback(msg):
if len(msg.poses) > 0:
global last_pose
last_pose = msg.poses[0]
if __name__ == '__main__':
rospy.init_node('posearray_tf_republisher')
sub = rospy.Subscriber('poses_out',PoseArray,callback)
br = tf.TransformBroadcaster()
try:
rate = rospy.Rate(10)
while not rospy.is_shutdown():
if not (last_pose is None):
(trans, rot) = pm.toTf(pm.fromMsg(last_pose))
br.sendTransform(trans, rot, rospy.Time.now(), 'drill', 'world')
rate.sleep()
except rospy.ROSInterruptException, e:
print e