def _transform_base_trajectory(self, base_traj, odom_to_frame_pose):
odom_to_frame = geometry.pose_to_tuples(odom_to_frame_pose)
num_points = len(base_traj.points)
odom_base_traj = JointTrajectory()
odom_base_traj.points = list(
utils.iterate(JointTrajectoryPoint, num_points))
odom_base_traj.header = base_traj.header
odom_base_traj.joint_names = self._base_joint_names
# Transform each point into odom frame
previous_theta = 0.0
for i in range(num_points):
t = base_traj.points[i].transforms[0]
frame_to_base = geometry.transform_to_tuples(t)
# odom_to_base = odom_to_frame * frame_to_base
(odom_to_base_trans, odom_to_base_rot) = geometry.multiply_tuples(
odom_to_frame, frame_to_base)
odom_base_traj.points[i].positions = [
odom_to_base_trans[0], odom_to_base_trans[1], 0
]
roll, pitch, yaw = T.euler_from_quaternion(odom_to_base_rot)
dtheta = geometry.shortest_angular_distance(previous_theta, yaw)
theta = previous_theta + dtheta
odom_base_traj.points[i].positions[2] = theta
previous_theta = theta
return odom_base_traj
def get_hand_poses(self, tf_buffer, offset=geometry.pose()):
ref_to_hands = []
for object_to_hand in self._object_to_hands:
ref_to_hands.append(
geometry.multiply_tuples(self._ref_to_object, object_to_hand))
if self._approach_frame == _HAND_FRAME:
ref_to_hand_offsets = []
for ref_to_hand in ref_to_hands:
ref_to_hand_offsets.append(
geometry.multiply_tuples(ref_to_hand, offset))
return ref_to_hand_offsets
else:
hand_to_approach_tf = tf_buffer.lookup_transform(
_HAND_FRAME,
self._approach_frame,
# rospy.get_rostime(),
rospy.Time(0),
rospy.Duration(1.0))
hand_to_approach = geometry.transform_to_tuples(
hand_to_approach_tf.transform)
ref_to_hand_offsets = []
for ref_to_hand in ref_to_hands:
ref_to_approach = geometry.multiply_tuples(
ref_to_hand, hand_to_approach)
ref_to_approach_offset = geometry.multiply_tuples(
ref_to_approach, offset)
ref_to_hand_offset = geometry.multiply_tuples(
ref_to_approach_offset, _inverse_tuples(hand_to_approach))
ref_to_hand_offsets.append(ref_to_hand_offset)
return ref_to_hand_offsets
def get_relative_tuples(base_frame, target_frame):
trans = whole_body._tf2_buffer.lookup_transform(base_frame,
target_frame,
rospy.Time(0),
rospy.Duration(_TF_TIMEOUT)).transform
tuples = geometry.transform_to_tuples(trans)
return tuples
def _transform_base_trajectory(self, base_traj):
"""Transform a base trajectory to an ``odom`` frame based trajectory.
Args:
base_traj (tmc_manipulation_msgs.msg.MultiDOFJointTrajectory):
A base trajectory
Returns:
trajectory_msgs.msg.JointTrajectory:
A base trajectory based on ``odom`` frame.
"""
odom_to_frame_transform = self._tf2_buffer.lookup_transform(
_BASE_TRAJECTORY_ORIGIN, base_traj.header.frame_id, rospy.Time(0),
rospy.Duration(_TF_TIMEOUT))
odom_to_frame = geometry.transform_to_tuples(
odom_to_frame_transform.transform)
num_points = len(base_traj.points)
odom_base_traj = JointTrajectory()
odom_base_traj.points = list(
utils.iterate(JointTrajectoryPoint, num_points))
odom_base_traj.header = base_traj.header
odom_base_traj.joint_names = self._base_client.joint_names
# Transform each point into odom frame
previous_theta = 0.0
for i in range(num_points):
t = base_traj.points[i].transforms[0]
frame_to_base = geometry.transform_to_tuples(t)
# odom_to_base = odom_to_frame * frame_to_base
(odom_to_base_trans, odom_to_base_rot) = geometry.multiply_tuples(
odom_to_frame, frame_to_base)
odom_base_traj.points[i].positions = [
odom_to_base_trans[0], odom_to_base_trans[1], 0
]
roll, pitch, yaw = T.euler_from_quaternion(odom_to_base_rot)
dtheta = geometry.shortest_angular_distance(previous_theta, yaw)
theta = previous_theta + dtheta
odom_base_traj.points[i].positions[2] = theta
previous_theta = theta
return odom_base_traj
def _lookup_odom_to_ref(self, ref_frame_id):
"""Resolve current reference frame transformation from ``odom``.
Returns:
geometry_msgs.msg.Pose:
A transform from robot ``odom`` to ``ref_frame_id``.
"""
odom_to_ref_ros = self._tf2_buffer.lookup_transform(
settings.get_frame('odom'), ref_frame_id, rospy.Time(0),
rospy.Duration(_TF_TIMEOUT)).transform
odom_to_ref_tuples = geometry.transform_to_tuples(odom_to_ref_ros)
return geometry.tuples_to_pose(odom_to_ref_tuples)
def get_relative_tuples(base_frame, target_frame):
while not rospy.is_shutdown():
try:
trans = whole_body._tf2_buffer.lookup_transform(
base_frame, target_frame, rospy.Time(0),
rospy.Duration(_TF_TIMEOUT)).transform
break
except ExtrapolationException as e:
continue
tuples = geometry.transform_to_tuples(trans)
return tuples
def get_end_effector_pose(self, ref_frame_id=None):
"""Get a pose of end effector based on robot frame.
Returns:
Tuple[Vector3, Quaternion]
"""
# Default reference frame is a robot frame
if ref_frame_id is None:
ref_frame_id = settings.get_frame('base')
transform = self._tf2_buffer.lookup_transform(
ref_frame_id, self._end_effector_frame, rospy.Time(0),
rospy.Duration(_TF_TIMEOUT))
result = geometry.transform_to_tuples(transform.transform)
return result
return res
def get_relative_tuples(base_frame, target_frame):
while not rospy.is_shutdown():
try:
trans = whole_body._tf2_buffer.lookup_transform(base_frame,
target_frame,
rospy.Time(0),
rospy.Duration(_TF_TIMEOUT)).transform
break
except ExtrapolationException as e:
continue
tuples = geometry.transform_to_tuples(trans)
return tuples
def grasp_point_callback(msg):
recog_pos.pose.position.x=msg.pose.position.x
recog_pos.pose.position.y=msg.pose.position.y
recog_pos.pose.position.z=msg.pose.position.z
# dkldsaflk;print recog_pos.pose.position
def grasp_point_client():
try:
print "calling grasping point service"
localize_handle = rospy.ServiceProxy('/track_handle',objectfinder)
global Is_found
Is_found=False
while(Is_found==False):
