def msg_to_kdl(msg): if isinstance(msg, TransformStamped): return transform_to_kdl(msg) elif isinstance(msg, PoseStamped): return pose_to_kdl(msg.pose) elif isinstance(msg, Pose): return pose_to_kdl(msg) elif isinstance(msg, PointStamped): return point_to_kdl(msg.point) elif isinstance(msg, Point): return point_to_kdl(msg) elif isinstance(msg, QuaternionStamped): return quaternion_to_kdl(msg.quaternion) elif isinstance(msg, Quaternion): return quaternion_to_kdl(msg) elif isinstance(msg, Twist): return twist_to_kdl(msg) elif isinstance(msg, TwistStamped): return twist_to_kdl(msg.twist) elif isinstance(msg, Vector3Stamped): return point_to_kdl(msg.vector) elif isinstance(msg, Vector3): return point_to_kdl(msg) else: raise TypeError(u'can\'t convert {} to kdl'.format(type(msg)))
def get_waypoints(self): start = None ti = 0 if self.current_trajectory is not None: ti = self.get_trajectory_index(self.current_trajectory, offset=1.0) if ti is None or ti == 0: ti = 0 try: trans = self.tfBuffer.lookup_transform( self.reference_frame, self.own_frame, rclpy.time.Time().to_msg()) frame = tf2_kdl.transform_to_kdl(trans) start = (frame.p.x(), frame.p.y(), frame.M.GetRPY()[2]) except Exception as e: self.get_logger().info(f"Exception in tf transformations\n{e}") else: start = (self.current_trajectory.poses[ti].pose.position.x, self.current_trajectory.poses[ti].pose.position.y, yaw_from_orientation( self.current_trajectory.poses[ti].pose.orientation)) goal = (self.goal.pose.position.x, self.goal.pose.position.y, yaw_from_orientation(self.goal.pose.orientation)) return [start, goal], ti
def get_initial_position(self, robot): if robot.simulation: robot_to_gob = (0.04, 0.08) else: robot_to_gob = robot.actuators.PUMPS.get(self.pump_id).get("pos") gob_to_robot = TransformStamped() gob_to_robot.transform.translation.x = -robot_to_gob[0] gob_to_robot.transform.translation.y = -robot_to_gob[1] goal_pose = TransformStamped() goal_pose.transform.translation.x = self.position[0] goal_pose.transform.translation.y = self.position[1] robot_pose = PoseStamped() robot_pose.pose.position.x = robot.get_position()[0] robot_pose.pose.position.y = robot.get_position()[1] angle = self.get_initial_orientation(robot) rot = Rotation.RotZ(angle) q = rot.GetQuaternion() goal_pose.transform.rotation.x = q[0] goal_pose.transform.rotation.y = q[1] goal_pose.transform.rotation.z = q[2] goal_pose.transform.rotation.w = q[3] gob_to_robot_kdl = transform_to_kdl(gob_to_robot) robot_goal_pose_kdl = do_transform_frame(gob_to_robot_kdl, goal_pose) return (robot_goal_pose_kdl.p.x(), robot_goal_pose_kdl.p.y())
def get_transfrom(reference_frame, target_frame): listener = tf.TransformListener() try: listener.waitForTransform(reference_frame, target_frame, rospy.Time(0), timeout=rospy.Duration(1)) translation_rotation = listener.lookupTransform( reference_frame, target_frame, rospy.Time()) except Exception as e1: try: tf_buffer = tf2_ros.Buffer() tf2_listener = tf2_ros.TransformListener(tf_buffer) transform_stamped = tf_buffer.lookup_transform( reference_frame, target_frame, rospy.Time(0), timeout=rospy.Duration(1)) translation_rotation = tf_conversions.toTf( tf2_kdl.transform_to_kdl(transform_stamped)) except Exception as e2: rospy.logerr("get_transfrom::\n " + "Failed to find transform from %s to %s" % ( reference_frame, target_frame, )) return translation_rotation
def _publish_tf(self, stamp): # check that we know which frames we need to publish from if self.map is None or self.base_frame is None: rospy.loginfo('Not publishing until map and odometry frames found') return # calculate the mean position x = np.mean([p.x for p in self.particles]) y = np.mean([p.y for p in self.particles]) theta = np.mean([p.theta for p in self.particles]) #TODO - wraparound # map to base_link map2base_frame = PyKDL.Frame( PyKDL.Rotation.Quaternion(*transformations.quaternion_from_euler(0, 0, theta)), PyKDL.Vector(x,y,0) ) odom2base_frame = tf2_kdl.transform_to_kdl(self.tf_buffer.lookup_transform( target_frame=self.odom_frame, source_frame=self.base_frame, time=stamp, timeout=rospy.Duration(4.0) )) # derive frame according to REP105 map2odom = map2base_frame * odom2base_frame.Inverse() br = tf2_ros.TransformBroadcaster() t = TransformStamped() t.header.stamp = stamp t.header.frame_id = self.map.frame t.child_frame_id = self.odom_frame t.transform.translation = Vector3(*map2odom.p) t.transform.rotation = Quaternion(*map2odom.M.GetQuaternion()) br.sendTransform(t) # for Debugging if False: t.header.stamp = stamp t.header.frame_id = self.map.frame t.child_frame_id = self.base_frame+"_old" t.transform.translation = Vector3(*map2base_frame.p) t.transform.rotation = Quaternion(*map2base_frame.M.GetQuaternion()) br.sendTransform(t) if self.publish_cloud: msg = PoseArray( header=Header(stamp=stamp, frame_id=self.map.frame), poses=[ Pose( position=Point(p.x, p.y, 0), orientation=Quaternion(*transformations.quaternion_from_euler(0, 0, p.theta)) ) for p in self.particles ] ) self.pose_array.publish(msg)
def _publish_tf(self, stamp): # check that we know which frames we need to publish from if self.map is None or self.base_frame is None: rospy.loginfo('Not publishing until map and odometry frames found') return # calculate the mean position x = np.mean([p.x for p in self.particles]) y = np.mean([p.y for p in self.particles]) theta = np.mean([p.theta for p in self.particles]) #TODO - wraparound # map to base_link map2base_frame = PyKDL.Frame( PyKDL.Rotation.Quaternion( *transformations.quaternion_from_euler(0, 0, theta)), PyKDL.Vector(x, y, 0)) odom2base_frame = tf2_kdl.transform_to_kdl( self.tf_buffer.lookup_transform(target_frame=self.odom_frame, source_frame=self.base_frame, time=stamp, timeout=rospy.Duration(4.0))) # derive frame according to REP105 map2odom = map2base_frame * odom2base_frame.Inverse() br = tf2_ros.TransformBroadcaster() t = TransformStamped() t.header.stamp = stamp t.header.frame_id = self.map.frame t.child_frame_id = self.odom_frame t.transform.translation = Vector3(*map2odom.p) t.transform.rotation = Quaternion(*map2odom.M.GetQuaternion()) br.sendTransform(t) # for Debugging if False: t.header.stamp = stamp t.header.frame_id = self.map.frame t.child_frame_id = self.base_frame + "_old" t.transform.translation = Vector3(*map2base_frame.p) t.transform.rotation = Quaternion( *map2base_frame.M.GetQuaternion()) br.sendTransform(t) if self.publish_cloud: msg = PoseArray( header=Header(stamp=stamp, frame_id=self.map.frame), poses=[ Pose(position=Point(p.x, p.y, 0), orientation=Quaternion( *transformations.quaternion_from_euler( 0, 0, p.theta))) for p in self.particles ]) self.pose_array.publish(msg)
def calculate_ik(base_link, tip_link, seed_joint_state, goal_transform_geometry_msg): """ Calculates the Inverse Kinematics from base_link to tip_link according to the given goal_transform_geometry_msg. The initial joint states would be considered from seed_joint_state. Returns the result joint states and the success status. base_link eg. - "triangle_base_link" or "calib_left_arm_base_link" or "calib_right_arm_base_link" tip_link eg. - "left_arm_7_link" or "right_arm_7_link" """ robot_urdf_string = rospy.get_param('robot_description') urdf_obj = urdf_parser_py.urdf.URDF.from_xml_string(robot_urdf_string) _, kdl_tree = kdl_parser_py.urdf.treeFromUrdfModel(urdf_obj) kdl_chain = kdl_tree.getChain(base_link, tip_link) num_joints = kdl_chain.getNrOfJoints() print "number of joints: " + str(num_joints) # Get Joint limits kdl_joint_limits_min, kdl_joint_limits_max = get_kdl_joint_limit_arrays( kdl_chain, urdf_obj) fk_solver = PyKDL.ChainFkSolverPos_recursive(kdl_chain) velocity_ik = PyKDL.ChainIkSolverVel_pinv(kdl_chain) # ik_solver = PyKDL.ChainIkSolverPos_LMA(kdl_chain, 1e-5, 1000, 1e-15) ik_solver = PyKDL.ChainIkSolverPos_NR_JL(kdl_chain, kdl_joint_limits_min, kdl_joint_limits_max, fk_solver, velocity_ik) # Getting the goal frame and seed state goal_frame_kdl = tf2_kdl.transform_to_kdl(goal_transform_geometry_msg) seed_joint_state_kdl = get_kdl_jnt_array_from_list(num_joints, seed_joint_state) # Solving IK result_joint_state_kdl = solve_ik(ik_solver, num_joints, seed_joint_state_kdl, goal_frame_kdl) # check if calculated joint state results in the correct end-effector position using FK goal_pose_reached = check_ik_result_using_fk(fk_solver, result_joint_state_kdl, goal_frame_kdl) # check if calculated joint state is within joint limits joints_within_limits = check_result_joints_are_within_limits( num_joints, result_joint_state_kdl, kdl_joint_limits_min, kdl_joint_limits_max) print "Result Joint State Within Limits: " + str(joints_within_limits) print "Can Reach Goal Pose With Solution: " + str(goal_pose_reached) result_joint_state_vector = get_list_from_kdl_jnt_array( num_joints, result_joint_state_kdl) goal_pose_reached_successfully = goal_pose_reached and joints_within_limits return result_joint_state_vector, goal_pose_reached_successfully
def get_transform(self, reference_frame, target_frame): try: transform_stamped = self.tfBuffer.lookup_transform( reference_frame, target_frame, rospy.Time(0), rospy.Duration(1.0)) translation_rotation = tf_conversions.toTf( tf2_kdl.transform_to_kdl(transform_stamped)) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException), e: print(e) rospy.logerr('FAILED TO GET TRANSFORM FROM %s to %s' % (reference_frame, target_frame)) return None
def get_waypoints(self): start = Pose2D() try: trans = self.tfBuffer.lookup_transform( self.reference_frame, self.own_frame, rclpy.time.Time().to_msg(), timeout=rclpy.time.Duration(seconds=3.0)) frame = tf2_kdl.transform_to_kdl(trans) start.theta = frame.M.GetRPY()[2] start.x = frame.p.x() start.y = frame.p.y() #self.get_logger().debug(f"got the following for ego position: {start}") except Exception as e: self.get_logger().info(f"Exception in tf transformations\n{e}") goal = Pose2D() goal.x = self.goal.pose.position.x goal.y = self.goal.pose.position.y goal.theta = yaw_from_orientation(self.goal.pose.orientation) return [start, goal]
def __init__(self): self.object_names = rospy.get_param( 'object_list', ['RedCylinder', 'BlueCuboid', 'GreenCube']) self.trans_noise = rospy.get_param('trans_noise', 0.0) self.rotation_noise = rospy.get_param('rotation_noise', 0.0) self.camera_link_name = rospy.get_param('camera_link_name', 'camera_link') self.object_publisher = rospy.Publisher('/recognized_objects_array', RecognizedObjectArray, queue_size=10) # Get camera link in reference to the world through tf self.tf_buffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) # tf publisher self.tf_pub = tf2_ros.TransformBroadcaster() self.matched_object_list = [] self.matched_object_lock = threading.Lock() rospy.sleep(5) cam_trans_msg = None while cam_trans_msg is None: try: cam_trans_msg = self.tf_buffer.lookup_transform( self.camera_link_name, 'world', rospy.Time.now()) finally: if cam_trans_msg is not None: break self.cam_trans = tf2_kdl.transform_to_kdl(cam_trans_msg) self.gazebo_subscriber = rospy.Subscriber('/gazebo/model_states', ModelStates, self.calback_gazebo_state, queue_size=30)
def odom_callback(self, msg): """Odom callback, computes the new pose of the robot relative to map, send this new pose on odom_map_relative topic and start vlx routine if this pose is near walls""" robot_tf = TransformStamped() robot_tf.transform.translation.x = msg.pose.pose.position.x robot_tf.transform.translation.y = msg.pose.pose.position.y robot_tf.transform.rotation = msg.pose.pose.orientation robot_frame = transform_to_kdl(robot_tf) new_robot_pose_kdl = do_transform_frame(robot_frame, self._initial_tf) self.robot_pose.pose.position.x = new_robot_pose_kdl.p.x() self.robot_pose.pose.position.y = new_robot_pose_kdl.p.y() q = new_robot_pose_kdl.M.GetQuaternion() self.robot_pose.header.stamp = msg.header.stamp self.robot_pose.header.frame_id = "map" self.robot_pose.pose.orientation = Quaternion(x=q[0], y=q[1], z=q[2], w=q[3]) self.odom_map_relative_publisher_.publish(self.robot_pose) if self.is_near_walls(self.robot_pose.pose.position): if self.vlx.continuous_sampling == 2: self.vlx.near_wall_routine(self.robot_pose) else: self.vlx.start_near_wall_routine(self.robot_pose) elif self.vlx.continuous_sampling == 2: self.vlx.stop_near_wall_routine()
for topic, msg, t in bag.read_messages(topics=['desired_pose']): trans_list.append(msg) for trans in trans_list: # Publish the desired tf trans.header.stamp = rospy.Time.now() broadcaster.sendTransform(trans) # Find your current cartesian pose youbot.forward_kinematics(youbot.current_joint_position, youbot.current_pose) youbot.broadcast_pose(youbot.current_pose) # Get next desired pose from rosbag frame = tf2_kdl.transform_to_kdl(trans) # Ikine jointkdl = youbot.inverse_kinematics_closed(frame) joint_array = [] for pos in jointkdl: joint_array.append(pos) print("publishing joint: {}".format(joint_array)) rospy.sleep(0.2) youbot.publish_joint_trajectory(joint_array) rospy.sleep(1) raw_input('Press enter for next pose\n')
def get_frame(parent, frame, time): return tf2_kdl.transform_to_kdl( tf2_buffer.lookup_transform(parent, frame, time))
frames = [] while not rospy.is_shutdown(): try: key = raw_input('Press Enter to read next frame ') if key == 'q': break trans_true = buffer.lookup_transform('tool_tracker', 'world', rospy.Time()) trans_vive = buffer.lookup_transform('world_vive', 'tracker_LHR_0DBB917B', rospy.Time()) frame_true = tf2_kdl.transform_to_kdl(trans_true) frame_vive = tf2_kdl.transform_to_kdl(trans_vive) world_vive = frame_vive * frame_true print('Current:', world_vive.p, world_vive.M.GetRPY()) frames.append(world_vive) xyz = [(f.p.x(), f.p.y(), f.p.z()) for f in frames] rpy = [f.M.GetRPY() for f in frames] print('Mean:', np.mean(xyz, axis=0).tolist(), np.mean(rpy, axis=0).tolist()) print('Std:', np.std(xyz, axis=0).tolist(), np.std(rpy, axis=0).tolist())