def _pose_sending_func(self):
'''
Continually sends the desired pose to the controller.
'''
r = rospy.Rate(100)
while not rospy.is_shutdown():
try:
r.sleep()
except rospy.ROSInterruptException:
break
# get the desired pose as a transform matrix
with self._state_lock:
ps_des = copy.deepcopy(self._desired_pose_stamped)
delta_dist = self._delta_dist
if ps_des == None:
# no desired pose to publish
continue
# convert desired pose to transform matrix from goal frame to CONTROLLER_FRAME
self._transform_listener.waitForTransform(
CONTROLLER_FRAME, ps_des.header.frame_id, rospy.Time(0), rospy.Duration(4.0))
#trans, rot = self._transform_listener.lookupTransform(CONTROLLER_FRAME, ps_des.header.frame_id, rospy.Time(0))
#torso_H_wrist_des = geom.trans_rot_to_hmat(trans, rot)
ps_des.header.stamp = rospy.Time(0)
ps_des = self._transform_listener.transformPose(CONTROLLER_FRAME, ps_des)
torso_H_wrist_des = conversions.pose_to_mat(ps_des.pose)
if self.br is not None:
p = ps_des.pose.position
q = ps_des.pose.orientation
self.br.sendTransform(
(p.x, p.y, p.z), (q.x, q.y, q.z, q.w),
rospy.Time.now(),
'/cci_goal',
CONTROLLER_FRAME)
# get the current pose as a transform matrix
goal_link = self._hand_description.hand_frame
# really shouldn't need to do waitForTransform here, since rospy.Time(0) means "use the most
# recent available transform". but without this, tf sometimes raises an ExtrapolationException. -jdb
self._transform_listener.waitForTransform(
CONTROLLER_FRAME, goal_link, rospy.Time(0), rospy.Duration(4.0))
trans, rot = self._transform_listener.lookupTransform(CONTROLLER_FRAME, goal_link, rospy.Time(0))
torso_H_wrist = geom.trans_rot_to_hmat(trans, rot)
# compute difference between actual and desired wrist pose
wrist_H_wrist_des = np.dot(linalg.inv(torso_H_wrist), torso_H_wrist_des)
trans_err, rot_err = geom.hmat_to_trans_rot(wrist_H_wrist_des)
# scale the relative transform such that the translation is equal
# to delta_dist. we're assuming that the scaled angular error will
# be reasonable
scale_factor = delta_dist / linalg.norm(trans_err)
if scale_factor > 1.0:
scale_factor = 1.0 # don't overshoot
scaled_trans_err = scale_factor * trans_err
angle_err, direc, point = transformations.rotation_from_matrix(wrist_H_wrist_des)
scaled_angle_err = scale_factor * angle_err
wrist_H_wrist_control = np.dot(
transformations.translation_matrix(scaled_trans_err),
transformations.rotation_matrix(scaled_angle_err, direc, point))
# find incrementally moved pose to send to the controller
torso_H_wrist_control = np.dot(torso_H_wrist, wrist_H_wrist_control)
desired_pose = conversions.mat_to_pose(torso_H_wrist_control)
desired_ps = PoseStamped()
desired_ps.header.stamp = rospy.Time(0)
desired_ps.header.frame_id = CONTROLLER_FRAME
desired_ps.pose = desired_pose
if self.br is not None:
p = desired_ps.pose.position
q = desired_ps.pose.orientation
self.br.sendTransform(
(p.x, p.y, p.z), (q.x, q.y, q.z, q.w),
rospy.Time.now(),
'/cci_imm_goal',
CONTROLLER_FRAME)
# send incremental pose to controller
self._desired_pose_pub.publish(desired_ps)
import roslib; roslib.load_manifest('pr2_python')
import rospy
from pr2_python.base import Base, _yaw
from pr2_python import transform_listener
from pr2_python import conversions
rospy.init_node('check_base_pose', anonymous=True)
H = transform_listener.get_transform('/map', '/base_footprint')
pose = conversions.mat_to_pose(H)
print (pose.position.x, pose.position.y, _yaw(pose.orientation))
def _pose_sending_func(self):
'''
Continually sends the desired pose to the controller.
'''
r = rospy.Rate(100)
while not rospy.is_shutdown():
try:
r.sleep()
except rospy.ROSInterruptException:
break
# get the desired pose as a transform matrix
with self._state_lock:
ps_des = copy.deepcopy(self._desired_pose_stamped)
delta_dist = self._delta_dist
if ps_des == None:
# no desired pose to publish
continue
# convert desired pose to transform matrix from goal frame to CONTROLLER_FRAME
self._transform_listener.waitForTransform(CONTROLLER_FRAME,
ps_des.header.frame_id,
rospy.Time(0),
rospy.Duration(4.0))
#trans, rot = self._transform_listener.lookupTransform(CONTROLLER_FRAME, ps_des.header.frame_id, rospy.Time(0))
#torso_H_wrist_des = geom.trans_rot_to_hmat(trans, rot)
ps_des.header.stamp = rospy.Time(0)
ps_des = self._transform_listener.transformPose(
CONTROLLER_FRAME, ps_des)
torso_H_wrist_des = conversions.pose_to_mat(ps_des.pose)
if self.br is not None:
p = ps_des.pose.position
q = ps_des.pose.orientation
self.br.sendTransform((p.x, p.y, p.z), (q.x, q.y, q.z, q.w),
rospy.Time.now(), '/cci_goal',
CONTROLLER_FRAME)
# get the current pose as a transform matrix
goal_link = self._hand_description.hand_frame
# really shouldn't need to do waitForTransform here, since rospy.Time(0) means "use the most
# recent available transform". but without this, tf sometimes raises an ExtrapolationException. -jdb
self._transform_listener.waitForTransform(CONTROLLER_FRAME,
goal_link, rospy.Time(0),
rospy.Duration(4.0))
trans, rot = self._transform_listener.lookupTransform(
CONTROLLER_FRAME, goal_link, rospy.Time(0))
torso_H_wrist = geom.trans_rot_to_hmat(trans, rot)
# compute difference between actual and desired wrist pose
wrist_H_wrist_des = np.dot(linalg.inv(torso_H_wrist),
torso_H_wrist_des)
trans_err, rot_err = geom.hmat_to_trans_rot(wrist_H_wrist_des)
# scale the relative transform such that the translation is equal
# to delta_dist. we're assuming that the scaled angular error will
# be reasonable
scale_factor = delta_dist / linalg.norm(trans_err)
if scale_factor > 1.0:
scale_factor = 1.0 # don't overshoot
scaled_trans_err = scale_factor * trans_err
angle_err, direc, point = transformations.rotation_from_matrix(
wrist_H_wrist_des)
scaled_angle_err = scale_factor * angle_err
wrist_H_wrist_control = np.dot(
transformations.translation_matrix(scaled_trans_err),
transformations.rotation_matrix(scaled_angle_err, direc,
point))
# find incrementally moved pose to send to the controller
torso_H_wrist_control = np.dot(torso_H_wrist,
wrist_H_wrist_control)
desired_pose = conversions.mat_to_pose(torso_H_wrist_control)
desired_ps = PoseStamped()
desired_ps.header.stamp = rospy.Time(0)
desired_ps.header.frame_id = CONTROLLER_FRAME
desired_ps.pose = desired_pose
if self.br is not None:
p = desired_ps.pose.position
q = desired_ps.pose.orientation
self.br.sendTransform((p.x, p.y, p.z), (q.x, q.y, q.z, q.w),
rospy.Time.now(), '/cci_imm_goal',
CONTROLLER_FRAME)
# send incremental pose to controller
self._desired_pose_pub.publish(desired_ps)
