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):